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Abstract 

During the last decade, sampling-based path planning algorithms, such as Probabilistic 
RoadMaps (PRM) and Rapidly-exploring Random Trees (RRT), have been shown to work 
well in practice and possess theoretical guarantees such as probabilistic completeness. However, 
little effort has been devoted to the formal analysis of the quality of the solution returned by 
such algorithms, e.g., as a function of the number of samples. The purpose of this paper is 
to fill this gap, by rigorously analyzing the asymptotic behavior of the cost of the solution re- 
turned by stochastic sampling-based algorithms as the number of samples increases. A number 
of negative results are provided, characterizing existing algorithms, e.g., showing that, under 
mild technical conditions, the cost of the solution returned by broadly used sampling-based 
algorithms converges almost surely to a non-optimal value. The main contribution of the paper 
is the introduction of new algorithms, namely, PRM* and RRT*, which are provably asymptot- 
ically optimal, i.e., such that the cost of the returned solution converges almost surely to the 
optimum. Moreover, it is shown that the computational complexity of the new algorithms is 
within a constant factor of that of their probabilistically complete (but not asymptotically op- 
timal) counterparts. The analysis in this paper hinges on novel connections between stochastic 
sampling-based path planning algorithms and the theory of random geometric graphs. 

Keywords: Motion planning, optimal path planning, sampling-based algorithms, random geometric graphs. 

1 Introduction 

The robotic motion planning problem has received a considerable amount of attention, especially 
over the last decade, as robots started becoming a vital part of modern industry as well as our 
daily life (Latombe, 1991; LaValle, 2006; Choset et al., 2005). Even though modern robots may 
possess significant differences in sensing, actuation, size, workspace, application, etc., the problem 
of navigating through a complex environment is embedded and essential in almost all robotics appli- 
cations. Moreover, this problem is relevant to other disciplines such as verification, computational 
biology, and computer animation (Latombe, 1999; Bhatia and Frazzoli, 2004; Branicky et al., 2006; 
Cortes et al., 2007; Liu and Badlcr, 2003; Finn and Kavraki, 1999). 

Informally speaking, given a robot with a description of its dynamics, a description of the 
environment, an initial state, and a set of goal states, the motion planning problem is to find a 
sequence of control inputs so as the drive the robot from its initial state to one of the goal states 
while obeying the rules of the environment, e.g., not colliding with the surrounding obstacles. An 
algorithm to address this problem is said to be complete if it terminates in finite time, returning a 
valid solution if one exists, and failure otherwise. 

Unfortunately, the problem is known to be very hard from the computational point of view. 
For example, a basic version of the motion planning problem, called the generalized piano movers 
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problem, is PSPACE-hard (Reif, 1979). In fact, while complete planning algorithms exist (see, e.g., 
Lozano-Perez and Wesley, 1979; Schwartz and Sharir, 1983; Canny, 1988), their complexity makes 
them unsuitable for practical applications. 

Practical planners came around with the development of cell decomposition methods (Brooks 
and Lozano-Perez, 1983) and potential fields (Khatib, 1986). These approaches, if properly im- 
plemented, relaxed the completeness requirement to, for instance, resolution completeness, i.e., 
the ability to return a valid solution, if one exists, if the resolution parameter of the algorithm 
is set fine enough. These planners demonstrated remarkable performance in accomplishing vari- 
ous tasks in complex environments within reasonable time bounds (Ge and Cui, 2002). However, 
their practical applications were mostly limited to state spaces with up to five dimensions, since 
decomposition-based methods suffered from large number of cells, and potential field methods from 
local minima (Koren and Borenstein, 1991). Important contributions towards broader applicabil- 
ity of these methods inchidc navigation functions (Rimon and Koditschek, 1992) and randomiza- 
tion (Barraquand and Latombc, 1993). 

The above methods rely on an explicit representation of the obstacles in the configuration 
space, which is used directly to construct a solution. This may result in an excessive computational 
burden in high dimensions, and in environments described by a large number of obstacles. Avoiding 
such a representation is the main underlying idea leading to the development of sampling-based 
algorithms (Kavraki and Latombe, 1994; Kavraki et al., 1996; LaValle and Kuffner, 2001). See 
Lindemann and LaValle (2005) for a historical perspective. These algorithms proved to be very 
effective for motion planning in high-dimensional spaces, and attracted significant attention over 
the last decade, including very recent work (see, e.g., Prentice and Roy, 2009; Tedrake et al., 
2010; Luders et al., 2010; Berenson et al., 2008; Yershova and LaValle, 2008; Stilman et al., 2007; 
Koyuncu et al., 2010). Instead of using an explicit representation of the environment, sampling- 
based algorithms rely on a collision checking module, providing information about feasibility of 
candidate trajectories, and connect a set of points sampled from the obstacle-free space in order 
to build a graph (roadmap) of feasible trajectories. The roadmap is then used to construct the 
solution to the original motion-planning problem. 

Informally speaking, sampling-based methods provide large amounts of computational savings 
by avoiding explicit construction of obstacles in the state space, as opposed to most complete motion 
planning algorithms. Even though these algorithms are not complete, they provide probabilistic 
completeness guarantees in the sense that the probability that the planner fails to return a solution, 
if one exists, decays to zero as the number of samples approaches infinity (Barraquand et al., 1997) 
(see also Hsu et al., 1997; Kavraki et al., 1998; Ladd and Kavraki, 2004). Moreover, the rate of decay 
of the probability of failure is exponential, under the assumption that the environment has good 
"visibility" properties (Barraquand et al., 1997). More recently, the empirical success of sampling- 
based algorithms was argued to be strongly tied to the hypothesis that most practical robotic 
applications, even though involving robots with many degrees of freedom, feature environments 
with such good visibility properties (Hsu et al., 2006). 

1.1 Sampling-Bcised Algorithms 

Arguably, the most influential sampling-based motion planning algorithms to date include Prob- 
abilistic RoadMaps (PRMs) (Kavraki et al., 1996, 1998) and Rapidly-exploring Random Trees 
(RRTs) (Kuffner and LaValle, 2000; LaValle and Kuffner, 2001; LaValle, 2006). Even though the 
idea of connecting points sampled randomly from the state space is essential in both approaches, 
these two algorithms differ in the way that they construct a graph connecting these points. 

The PRM algorithm and its variants are multiple-query methods that first construct a graph 
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(the roadmap), which represents a rich set of coUision-frcc trajectories, and then answer queries by 
computing a shortest path that connects the initial state with a final state through the roadmap. 
The PRM algorithm has been reported to perform well in high-dimensional state spaces (Kavraki 
et al., 1996). Furthermore, the PRM algorithm is probabilistically complete, and such that the 
probability of failure decays to zero exponentially with the number of samples used in the construc- 
tion of the roadmap (Kavraki et al., 1998). During the last two decades, the PRM algorithm has 
been a focus of robotics research: several improvements were suggested by many authors and the 
reasons to why it performs well in many practical cases were better understood (see, e.g., Branicky 
et al., 2001; Hsu et al., 2006; Ladd and Kavraki, 2004, for some examples). 

Even though multiple-query methods are valuable in highly structured environments, such as 
factory floors, most online planning problems do not require multiple queries, since, for instance, the 
robot moves from one environment to another, or the environment is not known a priori. Moreover, 
in some applications, computing a roadmap a priori may be computationally challenging or even 
infeasiblc. Tailored mainly for these applications, incremental sampling-based planning algorithms 
such as RRTs have emerged as an online, single-query counterpart to PRMs (see, e.g., Kuffner and 
LaValle, 2000; Hsu et al., 2002). The incremental nature of these algorithms avoids the necessity 
to set the number of samples a priori, and returns a solution as soon as the set of trajectories built 
by the algorithm is rich enough, enabling on-line implementations. Moreover, tree-based planners 
do not require connecting two states exactly and more easily handle systems with differential 
constraints. The RRT algorithm has been shown to be probabilistically complete (Kuffner and 
LaValle, 2000), with an exponential rate of decay for the probability of failure (Prazzoli et al., 
2002). The basic version of the RRT algorithm has been extended in several directions, and found 
many applications in the robotics domain and elsewhere (see, for instance, Prazzoli et al., 2002; 
Bhatia and Prazzoh, 2004; Cortes et al., 2007; Branicky et al., 2006, 2003; Zucker et al., 2007). In 
particular, RRTs have been shown to work effectively for systems with differential constraints and 
nonlinear dynamics (LaValle and Kuffner, 2001; Prazzoli et al., 2002) as well as purely discrete or 
hybrid systems (Branicky et al., 2003). Moreover, the RRT algorithm was demonstrated in major 
robotics events on various experimental robotic platforms (Bruce and Veloso, 2003; Kuwata et al., 
2009; Teller et al., 2010; Shkolnik et al., 2011; Kuffner et al., 2002). 

Other sampling-based planners of note include Expansive Space Trees (EST) (Hsu et al., 1997, 
1999) and Sampling-based Roadmap of Trees (SRT) (Plaku et al., 2005). The latter combines the 
main features of multiple-query algorithms such as PRM with those of single-query algorithms such 
as RRT and EST. 

1.2 Optimal Motion Planning 

In most applications, the quality of the solution returned by a motion planning algorithm is impor- 
tant. Por example, one may be interested in solution paths of minimum cost, with respect to a given 
cost functional, such as the length of a path, or the time required to execute it. The problem of 
computing optimal motion plans has been proven in Canny and Reif (1987) to be very challenging 

even in basic cases. 

In the context of sampling-based motion planning algorithms, the importance of computing op- 
timal solutions has been pointed out in early seminal papers (LaValle and Kuffner, 2001). However, 
optimality properties of sampling-based motion planning algorithms have not been systematically 
investigated, and most of the relevant work relics on heuristics. Por example, in many field imple- 
mentations of sampling-based planning algorithms (see, e.g., Kuwata et al., 2009), it is often the 
case that since a feasible path is found quickly, additional available computation time is devoted 
to improving the solution with heuristics until the solution is executed. Urmson and Simmons 
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(2003) proposed heuristics to bias the tree growth in RRT towards those regions that result in low- 
cost solutions. They have also shown experimental results evaluating the performance of different 
heuristics in terms of the quality of the solution returned. Ferguson and Stentz (2006) considered 
running the RRT algorithm multiple times in order to progressively improve the quality of the 
solution. They showed that each run of the algorithm results in a path with smaller cost, even 
though the procedure is not guaranteed to converge to an optimal solution. Criteria for restarting 
multiple RRT runs, in a different context, were also proposed in Wedge and Branicky (2008). A 
more recent approach is the transition-based RRT (T-RRT) designed to combine rapid exploration 
properties of the RRT with stochastic global optimization methods (Jaillet et al., 2010; Berenson 
et al., 2011). 

A different approach that also offers optimality guarantees is based on graph search algorithms, 
such as A*, apphed over a finite discretization (based, e.g., on a grid, or a cell decomposition of the 
configuration space) that is generated offline. Recently, these algorithms received a large amount of 
attention. In particular, they were extended to run in an anytime fashion (Likhachev et al., 2004, 
2008), deal with dynamic environments (Stentz, 1995; Likhachev et al., 2008), and handle systems 
with differential constraints (Likhachev and Ferguson, 2009). These have also been successfully 
demonstrated on various robotic platforms (Likhachev and Ferguson, 2009; Dolgov et al., 2009). 
However, optimality guarantees of these algorithms are only cnsTircd up to the grid resolution. 
Moreover, since the number of grid points grows exponentially with the dimensionality of the state 
space, so does the (worst-case) running time of these algorithms. 

1.3 Statement of Contributions 

To the best of the author's knowledge, this paper provides the first systematic and thorough analysis 
of optimality and complexity properties of the major paradigms for sampling-based path planning 
algorithms, for multiple- or single-query applications, and introduces the first algorithms that are 
both asymptotically optimal and computationally efficient, with respect to other algorithms in this 
class. A summary of the contributions can be found below, and is shown in Table 1. 

As a first set of results, it is proven that the standard PRM and RRT algorithms are not 
asymptotically optimal, and that the "simplified" PRM algorithm is asymptotically optimal, but 
computationally expensive. Moreover, it is shown that the A;-nearest variant of the (simplified) PRM 
algorithm is not necessarily probabilistically complete (e.g., it is not probabilistically complete for 
A; = 1), and is not asymptotically optimal for any fixed k. 

In order to address the limitations of sampling-based path planning algorithms available in the 
literature, new algorithms are proposed, i.e., PRM*, RRG, and RRT*, and proven to be probabilis- 
tically complete, asymptotically optimal, and computationally efficient. Of these, PRM* is a batch 
variable-radius PRM, applicable to multiple-query problems, in which the radius is scaled with the 
number of samples in a way that provably ensures both asymptotic optimality and computational 
efficiency. RRG is an incremental algorithm that builds a connected roadmap, providing similar 
performance to PRM* in a single-query setting, and in an anytime fashion (i.e., a first solution is 
provided quickly, and monotonically improved if more computation time is available). The RRT* al- 
gorithm is a variant of RRG that incrementally builds a tree, providing anytime solutions, provably 
converging to an optimal solution, with minimal computational and memory requirements. 

In this paper, the problem of planning a path through a connected bounded subset of a d- 
dimensional Euclidean space is considered. As in the early seminal papers on incremental sampling- 
based motion planning algorithms such as Kuffner and LaValle (2000) , no differential constraints are 
considered (i.e., the focus of the paper is on path planning problems), but our methods can be easily 
extended to planning in configuration spaces and applied to several practical problems of interest. 
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Table 1: Summary of results. Time and space complexity are expressed as a function of the number 
of samples n, for a fixed environment. 





Algorithm 


Probabilistic 


Asymptotic 


Monotone 


Time Complexity 


Space 




Completeness 


Optimality 


Convergence 


Processing 


Query 


Complexity 




PRM 


Yes 


No 


Yes 


0(n log n) 


0(n log n) 


0{n) 


tini 
it hi 


sPRM 


Yes 


Yes 


Yes 


0(n2) 


0(n2) 


0(n2) 


Exis 
Algor 


fc-sPRM 


Conditional 


No 


No 


0{n log n) 


0(n log n) 


0(n) 


RRT 


Yes 


No 


Yes 


0{n log n) 


0(n) 


0(n) 




PRM* 


Yes 


Yes 


No 


0{n log n) 


0(n log n) 


O(nlogn) 




fc-PRM* 


RRG 


Yes 


Yes 


Yes 


0(n log n) 


0(n log n) 


0(n log n) 


Pro] 
Algol 


fc-RRG 


RRT* 


Yes 


Yes 


Yes 


0{n log n) 


0(n) 


0{n) 




fc-RRT* 



The extension to systems with differential constraints is deferred to future work (see Karaman and 
Frazzoli (2010a) for preliminary results). 

Finally, the results presented in this article, and the techniques used in the analysis of the algo- 
rithms, hinge on novel connections established between sampling-based path planning algorithms 
in robotics and the theory of random geometric graphs, which may be of independent interest. 

A preliminary version of this article has appeared in Karaman and Frazzoli (2010b). Since 
then a variety of new algorithms based on the the ideas behind PRM*, RRG, and RRT* have been 
proposed in the literature. For instance, a probabilistically complete and probabilistically sound 
algorithm for solving a class of differential games has appeared in Karaman and Frazzoli (2010c). 
Algorithms based on the RRG were used to solve belief-space planning problems in Bry and Roy 
(2011). The RRT* algorithm was used for anytime motion planning in Karaman et al. (2011), 
where it was also demonstrated experimentally on a full-size robotic fork truck. In Alterovitz et al. 
(2011), the analysis given in Karaman and Frazzoli (2010b) was used to guarantee computational 
efficiency and asymptotic optimality of a new algorithm that can trade off between exploration and 
optimality during planning. 

A software library implementing the new algorithms introduced in this paper has been released 
as open-source software by the authors, and is currently available at http : / / ares . lids . mit . edu/ 
software/ 

1.4 Paper Organization 

This paper is organized as follows. Section 2 lays the ground in terms of notation and problem for- 
mulation. Section 3 is devoted to the discussion of the algorithms that are considered in the paper: 
first, the main paradigms for sampling-based motion planning algorithms available in the literature 
are presented, together with their main variants. Then, the new proposed algorithms are presented 
and motivated. In Section 4 the properties of these algorithms are rigorously analyzed, formally 
establishing their probabilistic completeness and asymptotically optimality (or lack thereof), as 
well as their computational complexity as a function of the number of samples and of the number 
of obstacles in the environment. Experimental results are presented in Section 5, to illustrate and 
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validate the theoretical findings. Finally, Section 6 contains conclusions and perspectives for future 
work. In order not to excessively disrupt the flow of the presentation, a summary of notation used 
throughout the paper, as well as lengthy proofs of important results are presented in the Appendix. 



2 Preliminary Material 

This section contains some preliminary material that will be necessary for the discussion in the 
remainder of the paper. Namely, the problems of feasible and optimal motion planning is intro- 
duced, and some important results from the theory of random geometric graphs are summarized. 
The notation used in the paper is summarized in Appendix A. 

2.1 Problem Formulation 

In this section, the feasible and optimal path planning problems are formalized. 

Let X = (0, 1)'^ be the configuration space, where d G N, d > 2. Let Xohs be the obstacle region, 
such that X \ Afobs is an open set, and denote the obstacle-free space as Xf^ee = cl(Af \ Xohs), where 
cl(-) denotes the closure of a set. The initial condition Xinit is an element of Affree, and the goal region 
Afgoai is an open subset of Affj-ce. A path planning problem is defined by a triplet (-^freej ^Jinit; -^goai)- 

Let a : [0, 1] — > W^; the total variation of a is defined as 



A function a with TV{a) < oo is said to have bounded variation. 

Definition 1 (Path) A function a : [0, 1] M"^ of bounded variation is called a 

• Path, if it is continuous; 

• Collision-free path, if it is a path, and o"(r) G Afree, for all r G [0, 1]; 

• Feasible path, if it is a collision-free path, a{0) = Xmit, and a{l) G cl(Afgoai). 

The total variation of a path is essentially its length, i.e., the Euclidean distance traversed by the 
path in M"^. The feasibility problem of path planning is to find a feasible path, if one exists, and 
report failure otherwise: 

Problem 2 (Feasible path planning) Given a path planning problem {Xfj-^e, Xinit, Xg^ai) > fii^d a 
feasible path a : [0, 1] X^j-^e such that a{0) = Xinit and a{l) G cl(Afgoai); if one exists. If no such 
path exists, report failure. 

Let S denote the set of all paths, and E^ee the set of all collision-free paths. Given two paths 
(Ti,(T2 G S, such that o"i(l) = 0-2(0), let ai\a2 G S denote their concatenation, i.e., (cri|(72)(r) := 
cji(2r) for all r G [0,1/2] and (o-i|cr2)(r) := cr2(2r - 1) for all r G (1/2,1]. Both E and Efrcc 
are closed under concatenation. Let c : S ^ R>o be a function, called the cost function, which 
assigns a strictly positive cost to all non-trivial collision- free paths (i.e., c{a) = if and only 
if cr(T) = (7(0), Vr G [0,1]). The cost function is assumed to be monotonic, in the sense that 
for all 0"i,cJ2 G S, c((Ti) < c((Ti[(T2), and bounded, in the sense that there exists kc such that 
c{a) < kcTY{a), Va G E. 

The optimality problem of path planning asks for finding a feasible path with minimum cost: 



n 



TV(a) 



sup 2^ 

{neN,0=ro<Ti<---<r„=s} ^^^^ 
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Problem 3 (Optimal path planning) Given a path planning problem (Affree, a^miti '^goai) o.'rtd a 
cost function c : S — M>o, find a feasible path a* such that c{a*) = mm{c{a) : a is feasible}. If no 
such path exists, report failure. 

2.2 Random Geometric Graphs 

The objective of this section is to summarize some of the results on random geometric graphs that 
are available in the literature, and are relevant to the analysis of sampling-based path planning 
algorithms. In the remainder of this article, several connections are made between the theory of 
random geometric graphs and path-planning algorithms in robotics, providing insight on a number 
of issues, including, e.g., probabilistic completeness and asymptotic optimality, as well as techni- 
cal tools to analyze the algorithms and establish their properties, hi fact, it turns out that the 
data structures constructed by most sampling-based motion planning algorithms in the literature 
coincide, in the absence of obstacles, with standard models of random geometric graphs. 

Random geometric graphs are in general defined as stochastic collections of points in a met- 
ric space, connected pairwise by edges if certain conditions (e.g., on the distance between the 
points) arc satisfied. Such objects have been studied since their introduction by Gilbert (1961); 
see, e.g., Penrose (2003) and Balister et al. (2009a) for an overview of recent results. From the the- 
oretical point of view, the study of random geometric graphs makes a connection between random 
graphs (BoUobas, 2001) and percolation theory (BoUobas and Riordan, 2006). On the application 
side, in recent years, random geometric graphs have attracted significant attention as models of ad 
hoc wireless networks (Gupta and Kumar, 1998, 2000). 

Much of the literature on random geometric graphs deals with infinite graphs defined on 
unbounded domains, with vertices generated as a homogeneous Poisson point process. Recall 
that a Poisson random variable of parameter A G R>o is an integer-valued random variable 
Poisson(A) : — >■ No such that P(Poisson(A) = k) = e~'^\^ /k\. A homogeneous Poisson point 
process of intensity A on is a random countable set of points C such that, for any disjoint 
measurable sets »Si,»S2 C M*^, 5i n 52 = 0, the numbers of points of Vf in each set are independent 
Poisson variables, i.e., card {T"^ H Si) = Poisson(^(5i)A) and card {T"^ n ^2) = Poisson(;u(52)A). In 
particular, the intensity of a homogeneous Poisson point process can be interpreted as the expected 
number of points generated in the unit cube, i.e., E(card {Vf n (0, 1)'')) = E(Poisson(A)) = A. 

Perhaps the most studied model of infinite random geometric graph is the following, introduced 
in Gilbert (1961), and often called Gilbert's disc model, or Boolean model: 

Definition 4 (Infinite random r-disc graph) Let A, r G M>0; and d G N. An infinite random 
r-disc graph G'^'^{X,r) in d dimensions is an infinite graph with vertices {Xi}i^^ = Vf, and such 
that {Xi,Xj), i,j G N, is an edge if and only if \\Xi — Xj\\ < r. 

A fundamental issue in infinite random graphs is whether the graph contains an infinite con- 
nected component, with non-zero probability. If it does, the random graph is said to percolate. 
Percolation is an important paradigm in statistical physics, with many applications in disparate 
fields such as material science, epidemiology, and microchip manufacturing, just to name a few (see, 
e.g., Sahimi, 1994). 

Consider the infinite random r-disc graph, for r = 1, i.e., G^'^'^(A, 1), and assume, without loss 
of generality, that the origin is one of the vertices of this graph. Let Pk{X) denote the probability 
that the connected component of G'^'^{X, 1) containing the origin contains k vertices, and define 
Poo (A) as Poo (A) = 1 - Y.'kLiPkW- The function poo : A Poo (A) is monotone, and poo(O) = 
and lim;s^^oo Poo (•^) = 1 (Penrose, 2003). A key result in percolation theory is that there exists a 
non-zero critical intensity Ac defined as Ac := sup{A : Poo(A) = 0}. In other words, for all A > Ac, 
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there is a non-zero probability that the origin is in an infinite connected component of G^'^^(A, 1); 
moreover, under these conditions, the graph has precisely one infinite connected component, almost 
surely (Meester and Roy, 1996). The function p^o is continuous for all A 7^ Ac: in other words, 
the graph undergoes a phase transition at the critical density Ac, often also called the continuum 
percolation threshold (Penrose, 2003). The exact value of Ac is not known; Meester and Roy 
provide 0.696 < Ac < 3.372 for d = 2 (Meester and Roy, 1996), and simulations suggest that 
Ac « 1.44 (Quintanilla et al., 2000). 

For many applications, including the ones in this article, models of finite graphs on a bounded 
domain are more relevant. Penrose introduced the following model (Penrose, 2003): 

Definition 5 (Random r-disc graph) Let r G K>o, and n,d e N. A random r-disc graph 
G'^^^'^{n,r) in d dimensions is a graph whose n vertices, {Xi,X2, . . ■ ,Xn}, are independent, uni- 
formly distributed random variables in (0,1)*^, and such that {Xi,Xj), i,j G {l,...,n}, i 7^ j, is 
an edge if and only if \\Xi — Xj\\ < r. 

For finite random geometric graph models, one is typically interested in whether a random geometric 
graph possesses certain properties asymptotically as n increases. Since the number of vertices is 
finite in random graphs, percolation can not be defined easily. In this case, percolation is studied 
in terms of the scaling of the number of vertices in the largest connected component with respect 
to the total number of vertices; in particular, a finite random geometric graph is said to percolate if 
it contains a "giant" connected component containing at least a constant fraction of all the nodes. 
As in the infinite case, percolation in finite random geometric graphs is often a phase transition 
phenomenon. In the case of random r-disc graphs. 

Theorem 6 (Percolation of random r-disc graphs (Penrose, 2003)) Let G'^^^'^{n,r) be a ran- 
dom r-disc graph in d > 2 dimensions, and let Naia,x{G'^^^'^{n,r)) be the number of vertices in its 
largest connected component. Then, almost surely. 



r iVma^(g^"'^(n,rn)) „ -f / \Vd 

hm ^ ^ = 0, ifrn < (Ac/n) ' , 

and 



r ^max(g<^"^(n,r)) ^ ^ ^ / ^/^ 

hm > 0, tfrn > (Ac/n) ' , 

n— ^-oo n 

where Ac is the continuum percolation threshold. 

A random r-disc graph with lim„^oo nrf^ = A G (0, co) is said to operate in the therm,odynamic 
limit. It is said to be in subcritical regime when A < Ac and supercritical regime when A > Ac. 

Another property of interest is connectivity. Clearly, connectivity implies percolation. Interest- 
ingly, emergence of connectivity in random geometric graphs is a phase transition phenomenon, as 
percolation. The following result is available in the literature: 

Theorem 7 (Connectivity of random r-disc graphs (Penrose, 2003)) Let G'^^^{n,r) be a 
random r-disc graph in d dimensions. Then, 



lirn^F (^{G'^^"^(n,r) is connected = <j 
where Q is the volume of the unit ball in d dimensions. 



1, if Qr"^ >^og{n)/n, 
0, if Cdr'^ < login) /n, 
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Another model of random geometric graphs considers edges between k nearest neighbors. (Note 
that there are no ties, almost surely.) Both infinite and finite models are considered, as follows. 



Definition 8 (Infinite random A;-nearest neighbor graph) Let A G M>o, and d,k E N. An 

infinite random k-nearest neighbor graph G^'^'^{X, k) in d dimensions is an infinite graph with 
vertices {Xjjjgpj = P^, and such that {Xi,Xj), i,j G N, is an edge if Xj is among the k nearest 
neighbors of Xi, or if Xi is among the k nearest neighbors of Xj. 

Definition 9 (Random A;-nearest neighbor graph) Let d,k,n ^ N. A random k-nearest neigh- 
bor graph G'^^^^{n,k) in d dimensions is a graph whose n vertices, {Xi,X2, ■ ■ ■ ,Xn}, are indepen- 
dent, uniformly distributed random variables in (0,1)*^, and such that {Xi,Xj), i,j G {!,..., n}, 
i ^ j, is an edge if Xj is among the k nearest neighbors of Xi, or if X^ is among the k nearest 
neighbors of Xj. 

Percolation and connectivity for random /c-nearest neighbor graphs exhibit phase transition 
phenomena, as in the random r-disc case. However, the results available in the literature are more 
limited. Results on percolation are only available for infinite graphs: 

Theorem 10 (Percolation in infinite random /c-nearest graphs (BaUster et al., 2009a)) 

Let G^^'^{X,k) be an infinite random k-nearest neighbor graph in d > 2 dimensions. Then, there 
exists a constant /c^ > such that 



' {{G^^'^{l,k) has an infinite component }) 



1, ifk>kl 
0, ifk<k^^. 



The value of k^ is not known. However, it is believed that fcg = 3, and k^ = 2 for all d > 3 (Balister 
et al., 2009a). It is known that percolation does not occur for A: = 1 (Balister et al., 2009a). 

Regarding connectivity of random /c- nearest neighbor graphs, the only available results in the 
literature are not stated in terms of a given number of vertices: rather, the results are stated 
in terms of the restriction of a homogeneous Poisson point process to the unit cube. In other 
words, the vertices of the graph are obtained as {Xi,X2, ...} = Vfr] (0, 1)'^. This is equivalent to 
setting the number of vertices as a Poisson random variable of parameter n, and then sampling the 
Poisson(n) vertices independently and uniformly in (0, 1)*^: 

Lemma 11 (Stoyan et al. (1995)) Let {Xi}i^^ be a sequence of points drawn independently 
and uniformly from 5 C Af. Let Poisson(n) be a Poisson random variable with parameter n. 
Then, {Xi, X2, . . . , ^Poisson(n)} is the restriction to S of a homogeneous Poisson point process with 
intensity n/fi{S). 

The main advantage in using such a model to generate the vertices of a random geometric graph 
is independence: in the Poisson case, the numbers of points in any two disjoint measurable regions 
(Si, 52 C [0, l]'^, iSi n 1S2 = 0, are independent Poisson random variables, with mean fi{Si)X and 
/i(52)A, respectively. These two random variables would not be independent if the total number 
of vertices were fixed a priori (also called a binomial point process). With some abuse of notation, 
such a random geometric graph model will be indicated as G"^^ (Poisson (n), k). 

Theorem 12 (Connectivity of random fc-nearest graphs (Balister et al., 2009b; Xue and Kumar, 20 

Let G'^^^^(Poisson(n), fe) indicate a k-nearest neighbor graph model in d = 2 dimensions, such that 
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its vertices are generated using a Poisson point process of intensity n. Then, there exists a constant 
> such that 

{1 if k> 
0, ifk<k^. 

The value of ^2 is not known; the current best estimate is 0.3043 < k2 < 0.5139 (Bahster et al., 
2005). 

Finally, the last model of random geometric graph that will be relevant for the analysis of the 
algorithms in this paper is the following: 

Definition 13 (Online nearest neighbor graph) Let d,n G N. An online nearest neighbor 

graph G^^^(n) in d dimensions is a graph whose n vertices, (Xi, X2, . . . , X„), are independent, 
uniformly distributed random variables in (0, 1)*^, and such that {Xi,Xj), i,j G {1, . . . ,n}, j > 1, 
is an edge if and only if \\Xi — Xj\\ = mini<fc<j \\Xk — Xj\\. 

Clearly, the online nearest neighbor graph is connected by construction, and trivially percolates. 
Recent results for this random geometric graph model include estimates of the total power- weighted 
edge length and an analysis of the vertex degree distribution, see, e.g.. Wade (2009). 



3 Algorithms 

In this section, a number of sampling-based motion planning algorithms are introduced. First, some 
common primitive procedures are defined. Then, the PRM and the RRT algorithms are outlined, as 
they are representative of the major paradigms for sampling-based motion planning algorithms in 
the literature. Then, new algorithms, namely PRM* and RRT*, are introduced, as asymptotically 
optimal and computationally efficient versions of their "standard" counterparts. 



3.1 Primitive Procedures 

Before discussing the algorithms, it is convenient to introduce the primitive procedures that they 
rely on. 



Sampling: Let Sample : uj i-^ {Samplej(a;)}igis}Q C X he a map from to sequences of points 
in X, such that the random variables Samplej, i G No, are independent and identically dis- 
tributed (i.i.d.). For simplicity, the samples are assumed to be drawn from a uniform distribu- 
tion, even though results extend naturally to any absolutely continuous distribution with density 
bounded away from zero on X. It is convenient to consider another map, SampleFree : ui 
{SampleFreej(a;)}igis}Q C Xfree that returns sequences of i.i.d. samples from Xf^ee- For each a; G $7, 
the sequence {SampleFreej(a;)}igNo is the subsequence of {Sainplej(a;)}jgN(, containing only the 
samples in -Yfree, i-e-, {SampleFreej(a;)}igNo = {Samplej(a;)}j6No ^ -Yfree- 

Nearest Neighbor: Given a graph G = (y,E), where V G X, a point x & X , the function 
Nearest : {G, x) v eV returns the vertex in V that is "closest" to x in terms of a given distance 
function. In this paper, the Euclidean distance is used (see, e.g., LaValle and Kuffner (2001) for 
alternative choices), and hence 

Nearest(G = {V,E),x) := argmin^gy — v\\. 
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A set-valued version of this function is also considered, kNearest : {G,x,k) i->- {vi,V2, . ■ ■ ,Vk}, 
returning the k vertices in V that are nearest to x, according to the same distance function as 
above. (By convention, if the cardinality of V is less than k, then the function returns V.) 

Near Vertices: Given a graph G = {V,E), where V C X, a point x e X, and a positive real 
number r G M>o, the function Near : (G, x,r) V' C. V returns the vertices in V that are 
contained in a ball of radius r centered at x, i.e., 

Near(G = {V, E),x,r) := {v e V : v e B^^^r} ■ 

Steering: Given two points x, y G , the function Steer : (z, y) i-^ z returns a point z & X 
such that z is "closer" to y than x is. Throughout the paper, the point z returned by the function 
Steer will be such that z minimizes Hz — y|| while at the same time maintaining ||z — < r], for 
a prespecified 77 > 0,^ i.e., 

Steer(x,y) := argmin^^g^ JI2; - y\\. 

Collision Test: Given two points x,x' G X, the Boolean function CollisionFree(a;, x') returns 
True if the line segment between x and x' lies in -Yfree, i-e., [x,x'] C -Yfree, and False otherwise. 

3.2 Existing Algorithms 

Next, some of the sampling-based algorithms available in the literature are outlined. For con- 
venience, inputs and outputs of the algorithms are not shown explicitly, but are as follows. All 
algorithms take as input a path planning problem (-^freej s^initj -^goai); an integer n G N, and a cost 
function c : S — t- M>o, if appropriate. These inputs are shared with functions and procedures called 
within the algorithms. All algorithms return a graph G = {V, E), where V C X^cc, card (V) < n+1, 
and -E G V X V. The solution of the path planning problem can be easily computed from such a 
graph, e.g., using standard shortest-path algorithms. 

Probabilistic RoadMaps (PRM): The Probabilistic RoadMaps algorithm is primarily aimed 
at multi-query applications. In its basic version, it consists of a pre-processing phase, in which 
a roadmap is constructed by attempting connections among n randomly-sampled points in Afree, 
and a query phase, in which paths connecting initial and final conditions through the roadmap 
are sought. "Expansion" heuristics for enhancing the roadmap's connectivity are available in the 
literature (Kavraki et al., 1996) but have no impact on the analysis in this paper, and will not be 
discussed. 

The pre-processing phase, outlined in Algorithm 1, begins with an empty graph. At each 

iteration, a point Xiand £ '^frcc is sampled, and added to the vertex set V. Then, connections are 
attempted between Xrand and other vertices in V within a ball of radius r centered at Xrandj in 
order of increasing distance from Xrand) using a simple local planner (e.g., straight-line connection). 
Successful (i.e., collision- free) connections result in the addition of a new edge to the edge set E. 
To avoid unnecessary computations (since the focus of the algorithm is establishing connectivity), 
connections between Xrand and vertices in the same connected component are avoided. Hence, the 
roadmap constructed by PRM is a forest, i.e., a collection of trees. 

'^This steering procedure is used widely in the robotics literature, since its introduction in Kuffner and LaValle 
(2000). Our results also extend to the Rapidly-exploring Random Dense Trees (see, e.g., LaValle, 2006), which are 
slightly modified versions of the RRTs that do not require tuning any prespecified parameters such as r] in this case. 
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Algorithm 1: PRM (preprocessing phase) 



1 V^^; E^^; 

2 for i = 0, . . . , n do 
Xrand SampleFreej; 
U ^ Near(G = (V, E),Xre,nd,r) ; 

V^VU {Xrand}; 

foreach u £ U, in order of increasing \\u — .Xrandll ! do 

if a^rand o-nd u are not in the same connected component of G = {V, E) then 

|_ if CollisionFree(Xrand, «) then E E U {(Xrand; a^rand)}; 



9 return G = {V,E)- 



Analysis results in the literature are only available for a "simplified" version of the PRM algo- 
rithm (Kavraki et al., 1998), referred to as sPRM in this paper. The simplified algorithm initializes 
the vertex set with the initial condition, samples n points from Affree, and then attempts to connect 
points within a distance r, i.e., using a similar logic as PRM, with the difference that connections 
between vertices in the same connected component are allowed. Notice that in the absence of 
obstacles, i.e., if Xf^ee = ^i the roadmap constructed in this way is a random r-disc graph. 

Algorithm 2: sPRM 

1 V ^ {xinit} U {SampleFreeJi=i,...,n; E 

2 foreach v E V do 

3 U ^near{G = {V,E),v,r)\{v}; 

4 foreach u & U do 

5 |_ if CollisionFree(f,«) then E E L) {{v,u), {u,v)} 

6 return G={V,Ey, 



Practical implementation of the (s)PRM algorithm have often considered different choices for 
the set U of vertices to which connections are attempted (i.e., line 4 in Algorithm 1, and line 3 in 
Algorithm 2). In particular, the following criteria are of particular interest: 

• fc-Nearest (s)PRM: Choose the nearest k neighbors to the vertex under consideration, 

for a given k (a typical value is reported as k = 15 (LaValle, 2006)). In other words, U 
kNearest(G = {V, E), Xj-and) k) in line 4 of Algorithm 1 and U ^ kNearest(G = {V, E),v, k)\ 
{v} in line 3 of Algorithm 2. The roadmap constructed in this way in an obstacle-free 
environment is a random fc-nearest graph. 

• Bounded-degree (s)PRM: For any fixed r, the average number of connections attempted 
at each iteration is proportional to the number of vertices in V, and can result in an excessive 
computational burden for large n. To address this, an upper bound k can be imposed on 
the cardinality of the set U (a typical value is reported as A; = 20 (LaValle, 2006)). In 
other words, U Near(G, Xrand) '') H kNearest(G, Xrand; ^) in line 4 of Algorithm 1, and 
U (Near(G, v, r) D kNearest(G, v, k)) \ {v} in line 3 of Algorithm 2. 

• Variable-radius (s)PRM: Another option to maintain the degree of the vertices in the 
roadmap small is to make the connection radius r a function of n, as opposed to a fixed 
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parameter. However, there are no clear indications in the hterature on the appropriate func- 
tional relationship between r and n. 

Rapidly-exploring Random Trees (RRT): The Rapidly-exploring Random Tree algorithm 
is primarily aimed at single-query applications. In its basic version, the algorithm incrementally 

builds a tree of feasible trajectories, rooted at the initial condition. An outline of the algorithm is 
given in Algorithm 3. The algorithm is initialized with a graph that includes the initial state as its 
single vertex, and no edges. At each iteration, a point Xrand £ 'Yfree is sampled. An attempt is made 
to connect the nearest vertex v G F in the tree to the new sample. If such a connection is successful, 
^^rand IS added to the vertex set, and (i", a^rand) is added to the edge set. In the original version 
of this algorithm, the iteration is stopped as soon as the tree contains a node in the goal region. 
In this paper, for consistency with the other algorithms (e.g., PRM), the iteration is performed n 
times. In the absence of obstacles, i.e., if Xbee = X-, the tree constructed in this way is an online 
nearest neighbor graph. 



Algorithm 3: RRT 


1 V ^ {xinit}; E^<D; 


2 for i = 1, . . . ,n do 


3 


a^rand ^ SampleFreGj; 


4 


Xnearest ^ Nearest(G = (V,-B),a;rand); 


5 


Xnew ^ Steer(a;nearest) •'^rand) j 


6 


if DbtacleFree(xnearcst-a^iicw) then 


7 


|_ F -(- 1^ U {Xnew}; -B -B U {(^nearest, aJnew)} ; 


8 return G = {V,Ey, 



A variant of RRT consists of growing two trees, respectively rooted at the initial state, and 
at a state in the goal set. To highlight the fact that the sampling procedure must not necessarily 
be stochastic, the algorithm is also referred to as Rapidly-exploring Dense Trees (RDT) (LaValle, 
2006). 



3.3 Proposed algorithms 

In this section, the new algorithms considered in this paper are presented. These algorithms 
are proposed as asymptotically optimal and computationally efficient versions of their "standard" 
counterparts, as will be made clear through the analysis in the next section. Input and output data 
are the same as in the algorithms introduced in Section 3.2. 

Optimal Probabilistic RoadMaps (PRM*): In the standard PRM algorithm, as well as in its 
simplified "batch" version considered in this paper, connections are attempted between roadmap 
vertices that are within a fixed radius r from one another. The constant r is thus a parameter of 
PRM. The proposed algorithm — shown in Algorithm 4 — is similar to sPRM, with the only diff'erence 
being that the connection radius r is chosen as a function of n, i.e., r = r{n) := 7PRM(log(n)/n)^/'^, 

where 7prm > 7prm — ^(1 + 1/c?)^^'^ (//(Affree)/C<i)^'''^) d is the dimension of the space X, /Lt(Affree) 
denotes the Lebesgue measure (i.e., volume) of the obstacle-free space, and Q is the volume of the 
unit ball in the d-dimensional Euclidean space. Clearly, the connection radius decreases with the 
number of samples. The rate of decay is such that the average number of connections attempted 
from a roadmap vertex is proportional to log(n). 
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Note that in the discussion of variable-radius PRM in LaValle (2006), it is suggested that the 
radius be chosen as a function of sample dispersion. (Recall that the dispersion of a point set 
contained in a bounded set 5 C M'' is the radius of the largest empty ball centered in S.) Indeed, 
the dispersion of a set of n random points sampled uniformly and independently in a bounded set 
is 0((log(n)/n)^/'^) (Niederreiter, 1992), which is precisely the rate at which the connection radius 
is scaled in the PRM* algorithm. 



Algorithm 4: PRM* 


1 V {xinit} U {SampleFreej}i=i,...,„; 




2 foreach v €^ V do 




3 


U ^ Near(G = {V, E),v,jprm{Io 


g(n)/n)V'^) \ {v}; 


4 


foreach u ^ U do 




5 


1^ if CollisionFree(?;, «) then 


E <(- Eyj{{v,u), {u,v)} 


6 return G={V,E); 





Another version of the algorithm, called A;- nearest PRM*, can be considered, motivated by the 
/c-nearest PRM implementation previously mentioned, whereby the number k of nearest neighbors 
to be considered is not a constant, but is chosen as a function of the cardinality of the roadmap n. 
More precisely, k{n) := A:pRMlog(ra), where /cprm > ^prm = e(l + 1/d), and U kNearest(G = 
{V, E),v, /cpRM log(n)) \ {v} in line 3 of Algorithm 4. 

Note that kp^y^ is a constant that only depends on d, and does not otherwise depend on the 
problem instance, unlike Tprm- Moreover, A;pr,m = 2e is a valid choice for all problem instances. 

Rapidly-exploring Random Graph (RRG): The Rapidly-exploring Random Graph algo- 
rithm was introduced as an incremental (as opposed to batch) algorithm to build a connected 
roadmap, possibly containing cycles. The RRG algorithm is similar to RRT in that it first at- 
tempts to connect the nearest node to the new sample. If the connection attempt is successful, the 
new node is added to the vertex set. However, RRG has the following difference. Every time a new 
point Xnew is added to the vertex set V, then connections are attempted from all other vertices in 
V that are within a ball of radius r(card(F)) = min{7RRG(log(card (y))/ card (F))-^/*^, r/}, where 
rj is the constant appearing in the definition of the local steering function, and 7rrg > 7rrg ~ 
2 (1 -|- l/dY^'^ {lJ'i^free)/Cd)^^^ ■ For each successful connection, a new edge is added to the edge set 
E. Hence, it is clear that, for the same sampling sequence, the RRT graph (a directed tree) is a 
subgraph of the RRG graph (an undirected graph, possibly containing cycles). In particular, the 
two graphs share the same vertex set, and the edge set of the RRT graph is a subset of that of the 
RRG graph. 

Another version of the algorithm, called A;-nearest RRG, can be considered, in which connections 
are sought to k nearest neighbors, with k = A;(card(y)) := /crrg log(card (F)), where A;rrg > 
^RRG = e(l 1/d), and Xnear ^ kNearest(G = (V, £■), Xnew, ^rrg log(card (F))), in line 7 of 
Algorithm 5. 

Note that k^^^Q is a constant that depends only on d, and does not depend otherwise on the 
problem instance, unlike 7rrg- Moreover, ^rrg = 2e is a valid choice for all problem instances. 

Optimal RRT (RRT*): Maintaining a tree structure rather than a graph is not only economical 
in terms of memory requirements, but may also be advantageous in some applications, due to, for 
instance, relatively easy extensions to motion planning problems with differential constraints, or 
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Algorithm 5: RRG 



1 

2 

3 
4 
5 
6 
7 
8 
9 
10 



V ^ {xinit}; E^$- 
for i = 1, . . . , n do 

^^rand ^ SampleFreGj; 
^nearest ^ Nearest(G = (y, £;),a;rand); 

'''new ^ Steer(Xnearest) •''rand) ! 
if ObtacleFree(xnearest, Xnew) then 

Xnear ^ Near(G = (l^, £;), Xncw, min{7RRG (log(card {V))/ card (1/))^/^ ??}) ; 

^ ^ U {^new}) E i E U {(^nearest) ^new); (^^new) ^^nearest)} ; 
foreach Xnear e Xnear do 
1^ if CollisionFree(Xnear, a^new) then E E\J {(Xnear, iCnew), (iCnew, a:near)} 



11 return G = {V,E)] 



to cope with modeling errors. The RRT* algorithm is obtained by modifying RRG in such a way 
that formation of cycles is avoided, by removing "redundant" edges, i.e., edges that are not part 
of a shortest path from the root of the tree (i.e., the initial state) to a vertex. Since the RRT and 
RRT* graphs are directed trees with the same root and vertex set, and edge sets that are subsets 
of that of RRG, this amounts to a "rewiring" of the RRT tree, ensuring that vertices are reached 
through a minimum-cost path. 

Before discussing the algorithm, it is necessary to introduce a few new functions. Given two 
points xi,X2 G M*^, let Line(a;i,X2) : [0, s] — t- X denote the straight-line path from xi to .T2. Given 
a tree G = {V, E), let Parent : V ^ V he a function that maps a vertex v & V to the unique vertex 
u e V such that {u,v) G E. By convention, ii vq & V is the root vertex of G, Parent(t;o) = vq. 
Finally, let Cost : V M>o be a function that maps a vertex v e V to the cost of the unique 
path from the root of the tree to v. For simplicity, in stating the algorithm we will assume an 
additive cost function, so that Cost(f) = Cost(Parent(?;)) + c(Line(Parent(t;), ?;)), although this 
is not necessary for the analysis in the next section. By convention, if vq E V is the root vertex of 
G, then Cost('yo) = 0. 

The RRT* algorithm, shown in Algorithm 6, adds points to the vertex set V in the same way 
as RRT and RRG. It also considers connections from the new vertex Xncw to vertices in X^^^m-, i-C-; 
other vertices that are within distance r(card(y)) = min{7RRT* (log(card (y))/ card (F))^/'^, r/} 
from Xnew- However, not all feasible connections result in new edges being inserted in the edge set 
E. In particular, (i) an edge is created from the vertex in Xncar that can be connected to x^cw 
along a path with minimum cost, and (ii) new edges are created from Xncw to vertices in Xncar^ if 
the path through Xnew has lower cost than the path through the current parent; in this case, the 
edge linking the vertex to its current parent is deleted, to maintain the tree structure. 

Another version of the algorithm, called fc-nearest RRT*, can be considered, in which con- 
nections are sought to k nearest neighbors, with A;(card(y)) = /crrg log (card (1^)), and Xnear 
kNearest(G = (V, E),Xnew, ^RRG log(i)), in line 7 of Algorithm 6. 

4 Analysis 

In this section, a number of results concerning the probabilistic completeness, asymptotic optimal- 
ity, and complexity of the algorithms in Section 3 are presented. 

The return value of Algorithms 1-6 is a graph. Since the sampling procedure SampleFree is 
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Algorithm 6: RRT* 



1 

2 
3 
4 
5 
6 
7 
8 
9 
10 
11 
12 

13 
14 
15 

16 



V ^ {xinit}; ^ ^ 0; 
for f = 1 , . . . , n do 

a^rand ^ SampleFreGj; 
a^nearest ^ Nearest(G = (y, £;),a;rand); 

a^new ^ Steer(a;nearest) ajj-and) ! 
if ObtacleFree(xnearest,a;new) then 

Xnear ^ Near(G = {V, Xnew, min{7RRT. (log(card (F))/ card (1/))^/^ ry}) ; 
y ^ FU{x„ew}; 

a^min ^ a^nearest) Cmin ^ Cost(Xnearest) "l~ c(Line(Xnearest) aJnew))) 

foreach Xncar £ ^ncar do // Conncct along a minimum-cost path 

if CollisionFree(xncar,a;new) A Cost(xncar) + c(Line(xncar,a;new)) < Cmin then 

^ ^mm ^ a^ncar! Cmin ^ CoS't(Xnear) ~l~ c(Lilie(Xnear) a^new)) 



E -(^ EVJ {(Xmin, Xncw)}; 
foreach Xnear G -'^near do 

if CollisionFree(Xnew, iCnear 

then Xparent Parent (xncar) ; 

E -(^ {E\ {(Xparent,a;near)}) U {(X; 



A Cost(xnew) + c(Line(2;n 

/) Senear)} 



1 1 Rewire the tree 

ar )) < Cost(Xnear) 



17 return G = {V,E)- 



stochastic, the returned graph is in fact a random variable.^ Since the sampling procedure is 
modeled as a map from the sample space to infinite sequences in X, sets of vertices and edges 
of the graphs maintained by the algorithms can be defined as functions from the sample space 
to appropriate sets. More precisely, let ALG be a label indicating one of the algorithms in Section 
3, and let {V/^^'~^{u))}i^f>i and {Ef^^^ {ui)}i^f^ be, respectively, the sets of vertices and edges in the 
graph returned by algorithm ALG, indexed by the number of samples, for a particular realization 
of the sample sequence. (In other words, these are sequences of functions defined from Q into finite 
subsets of Afree Or Af^ee X X^.^^.) Similarly, let Gf^G = (yALG^^ALG^_ (The label ALG will be at 
times omitted when the algorithm being used is clear from the context.) 

All algorithms considered in the paper are sound, in the sense that they only return graphs 
with vertices and edges representing points and paths in A'fi-ee-This statement can be easily verified 
by inspection of the algorithms in Section 3. 

4.1 Probabilistic Completeness 

In this section, the feasibility problem is considered, and the (probabilistic) completeness properties 
of the algorithms in Section 3 are analyzed. First, some preliminary definitions are given, followed 
by a definition of probabilistic completeness. Then, completeness properties of various sampling- 
based motion planning algorithms are stated. 

Let (5 > be a real number. A state x £ ^^tee is said to be a S-interior state of Afree, if the closed 
ball of radius 6 centered at x lies entirely inside Xfree- The S-interior of Xfree, denoted as int5(A'fi.ee), 
is defined as the collection of all (5-interior states, i.e., mts{X{ree) '■= € -^free \ Sx,d ^ -^free}- In 

^ Wo will not address the case in which the sampling procedure is deterministic, but refer the reader to LaValle et al. 
(2004), which contains an in-depth discussion of the relative merits of randomness and determinism in sampling-based 
motion planning algorithms. 
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Figure 1: An illustration of the (5-interior of Affree- The obstacle region Afobs is shown in dark grey 
and the (5-interior of -Yfree is shown in light grey. The distance between the dashed boundary of 
mts{X{ree) and the solid boundary of -Yfree is precisely S. 

other words, the 5-intcrior of Aficc is the set of all states that are at least a distance 5 away from any 
point in the obstacle set (see Figure 1). A collision-free path a : [0, 1] — )■ Affree is said to have strong 
S-clearance, if a lies entirely inside the 5-interior of Xf^ee, i-e., cr(T) G int5(^free) for all r G [0, 1]. A 
path planning problem (-^free; s^init) -^goai) is said to be robustly feasible if there exists a path with 
strong (5-clcarance, for some S > 0, that solves it. In terms of the notation used in this paper, the 
notion of probabilistic completeness can be stated as follows. 

Definition 14 (Probabilistic Completeness) An algorithm ALG is probabilistically complete, 
if, for any robustly feasible path planning problem (Afree, a^init, -^goai); 

liminf P ({3xgoai € V^^'^ fl Xsaai such that Xinit is connected to Xgoai if*- G^*^ }) = 1. 

If an algorithm is probabilistically complete, and the path planning problem is robustly feasible, 
the limit 

lim^^oo f ({3a;goai G Vn^'^^ ^ '^goai such that a;init is connected to Xgoai in G^^*^ }) 

exists and is equal to 1. On the other hand, the same limit is equal to zero for any sampling-based 
algorithm (including probabilistically complete ones) if the problem is not robustly feasible, unless 
the samples are drawn from a singular distribution adapted to the problem. 

It is known from the literature that the sPRM and RRT algorithms are probabilistically com- 
plete, and that the probability of finding a solution if one exists approaches one exponentially fast 
with the number of vertices in the graph returned by the algorithms. In other words. 

Theorem 15 (Probabilistic completeness of sPRM (Kavraki et al., 1998)) Consider a ro- 
bustly feasible path planning problem (Affrcc, Xinit, -^goai)- There exist constants a > and uq G N, 
dependent only on ^free o-'^'d '^goai? such that 

P ({3 Xgoai G Vf^^ n -Ygoai : Xgoai is Connected to Xi„it in Gf^^}) > 1 - e"""*, Vn > no. 

Theorem 16 (Probabilistic Completeness of RRT (LaValle and Kuffner, 2001)) Consider 
a robustly feasible path planning problem (Afree, iCinitj -^goai)- There exist constants a > and 
no G N, both dependent only on Xfree o-nd Xg^oaX, such that 

P ({V;^^T n A-goai / 0}) > 1 - e-«", Vn > no. 

On the other hand, the probabilistic completeness results do not necessarily extend to the 
heuristics used in practical implementations of the (s)PRM algorithm, as detailed in Section 3. For 
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example, consider the A;- nearest sPRM algorithm, where k = 1. That is, each vertex is connected 
to its nearest neighbor and the resulting undirected graph is returned as the output. This sPRM 
algorithm will be called the 1-nearest sPRM, and indicated with the label IPRM. The RRT 
algorithm can be thought of as the incremental version of the 1-nearest sPRM algorithm: the RRT 
algorithm also connects each sample to its nearest neighbor, but forces connectivity of the graph by 
an incremental construction. The following theorem shows that the 1-nearest sPRM algorithm is 
not probabilistically complete, although the RRT is (see Theorem 16). Furthermore, the probability 
that it fails to find a path converges to one as the number of samples approaches infinity. 

Theorem 17 (Incompleteness of /c-nearest sPRM for A; = 1) The k-nearest sPRM algorithm 
is not probabilistically complete for k = 1. Furthermore, 

lim P ({3xgoai € V^^^^'^ n A'goai such that Xinit is connected to Xgoai in G^^*^ }) = 0. 

The proof of this theorem requires two intermediate results that are provided below. For simplicity 
of presentation, consider the case when Xf^^c = X- Let G^f^^ = {V^^^^ , E}^^^) denote the 
graph returned by the 1-nearest sPRM algorithm, when the algorithm is run with n samples. Let 
Ln denote the total length of all the edges present in G^^^ . Recall that C,d denotes the volume of 
the unit ball in the d-dimensional Euclidean space. Let C,'^ denote the volume of the union of two 
unit balls whose centers are a unit distance apart. 

Lemma 18 (Total length of the 1-nearest neighbor graph (Wade, 2007)) For all d > 2, 

Ln/n^~^^^ converges to a constant in mean square, i.e.. 



hm E 

n— )-oo 



= 0. 



,ni-i/d V dj 2(C^)i+V'i^ 
Proof This lemma is a direct consequence of Theorem 3 of Wade (2007). □ 
Let Nn denote the number of connected components of G^^^. 

Lemma 19 (Number of connected components of the 1-nearest neighbor graph) For all 

d>2, Nn/n converges to a constant in mean square, i.e.. 



lim E 

n—^oo 



0. 



Proof A reciprocal pair is a pair of vertices each of which is the other one's nearest neighbor. In a 
graph formed by connecting each vertex to its nearest neighbor, any connected component includes 
exactly one reciprocal pair whenever the number of vertices is greater than 2 (see, e.g., Eppstein 
et al., 1997). The number of reciprocal pairs in such a graph was shown to converge to C^C'd) 
mean square in Henze (1987) (see also Remark 2 in Wade (2007)). □ 

Proof of Theorem 17 Let L„ denote the average length of a connected component in G^^^, i.e., 
Ln = Ln/Nn. Let L'n denote the length of the connected component that includes xinit- Since the 
samples are drawn independently and uniformly, the random variables L„ and L'^ have the same 
distribution (although they are clearly dependent). Let denote the constant that L„/n^^^/'^ 
converges to (see Lemma 18). Similarly, let 7jv denote the constant that Nn/n converges to (see 
Lemma 19). 
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Recall that convergence in mean square implies convergence in probability and hence conver- 
gence in distribution (Grimmctt and Stirzaker, 2001). Since both Ln/n^-i/"' and iV„/n converge 
in mean square to constants and P({A^„ = 0}) = for all n G N, by Slutsky's theorem (Rcsnick, 

1999), n ' Ln = "j^ 1^ — converges to 7 := ^l/in in distribution. In this case, it also converges 

in probability, since 7 is ajjonstant (Grimmett and Stirzaker, 2001). Then, v}/'^ L'^ also converges 
to 7 in probability, since L„ and L'^ are identically distributed for all n G N. Thus, L'^ converges 
to in probability, i.e., lim„_!.oo P ({-^n > e}) = 0, for all e > 0. 

Let e > be such that e < inix^x^oai ll^^^^initH- Let An denote the event that the graph returned 
by the 1-nearest sPRM algorithm contains a feasible path, i.e., one that starts from xinit and reaches 
the goal region Clearly, the event {L^ > e} occurs whenever does, i.e.. An C {L^ > e}. Then, 
P(-4n) < ^{{L'n > e}). Taking the limit superior of both sides 

liminfP(An) < limsupP(^„) < limsupP({L[j > e}) = 0. 

"■"^"^ n—^oo n—^oo 

In other words, the limit lim„_>oo P(^n) exists and is equal zero. □ 

Consider the variable-radius sPRM algorithm. The following theorem asserts that variable- 
radius sPRM algorithm is not probabilistically complete in the subcritical regime. 

Theorem 20 (Incompleteness of variable-radius sPRM with r(n) = 7n~^/'^) There exists a 
constant 7 > such that the variable radius sPRM with connection radius r{n) = ^n"^^'^ is not 
probabilistically complete. 

The proof of this result requires some intermediate results from random geometric graph theory. 

Recall that Ac is the critical density, or continuum percolation threshold (see Section 2.2). Given a 
Borel set F C R"', let G^^'^{n,r) denote the random r-disc graph formed with vertices independent 
and uniformly sampled from F and edges connecting two vertices, v and v', whenever ||?; — < r„. 

Lemma 21 (Penrose (2003)) Let X € (0, Ac) and T C M."^ be a Borel set. Consider a sequence 
{fn}nen thO't satisfies nr^ < X, Mn ^ N. Let N,^^{GY^'^{n,rn)) denote the size of the largest 
component in G'^^^{n,rn). Then, there exist constants a,b > and mo G N such that for all 
m > mo, 

P {{Nm^G^'^n, rn)) > m}) < n (e""- + e""") . 

Proof of Theorem 20 Let e > such that e < inf-rgXgoai ||3J~^init|| and that the 2e-ball centered 
at Xinit lies entirely within the obstacle-free space. Let G^^^ = {Vn^^,En^^) denote the graph 
returned by this variable radius sPRM algorithm, when the algorithm is run with n samples. 
Let Gn = {Vn,En) denote the the restriction of Gn^^ to the 2e-ball centered at Xinit defined as 

Vn = V^^^ n B,,^,,,2e and En = {Vn X Vn) H E^^^. 

Clearly, G„ is equivalent to the random r-disc graph on P = Bxi^i^,2e■ Let NmaxiGn) denote the 
number of vertices in the largest connected component of Gn- By Lemma 21, there exists constants 
a,b > and mo G N such that 

P({A^ma.(G„) >m})<n (e"""* + e"''") , 
for all m > mo- Then, for all m = X~^/'^ (e/2) n^l'^ > mo, 
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Let Ln denote the total length of all the edges in the connected component that includes xinit- 
Since r„ = X^/'^n'^/'^, 

Since the right hand side is summable, by the Borcl-Cantelli lemma the event {-L„ > e/2} occurs 
infinitely often with probability zero, i.e., P(limsup„^oQ{L„ > e/2}) = 0. 

Given a graph G = {V^ E) define the diameter of this graph as the distance between the farthest 
pair of vertices in V , i.e., maxy .^'gy — "y'H. Let £)„ denote the diameter of the largest component 
in Gn- Clearly, < L„ holds surely. Thus, P (limsup^^o^ {D„ > e/2}) = 0. 

Let / G N be the smallest number that satisfies r/ < e/2. Notice that the edges connected 
to the vertices l^^'^ n Bx-^^^^^e coincide with those connected to T^i Pi Bx-^^^^^e-, for all n > I. Let 
Rn denote distance of the farthest vertex v G V^^^ to Xinit in the component that contains Xinit 
in G^^^^^. Notice also that i?„ > e only if Dn > e/2, for all n > I. That is, for all n > /, 
{Rn > e} C {Dn > e/2}, which implies P (hmsup„^^ {Rn > e}) = 0. 

Let An denote the event that the graph returned by this variable radius sPRM algorithm 
includes a path that reaches the goal region. Clearly, {Rn > e} holds, whenever An holds. Hence, 
IP(^n) < lP({-Rn > e})- Taking the limit superior of both sides yields 

liminfP(A,) < limsupP(^„) < limsupP ({i?„ > e}) < p( limsup {i2„ > e} ) = 0. 

Hence, lim„_^oo = 0. □ 

Finally, the probabilistic completeness of the new algorithms proposed in Section 3 is established. 
Probabilistic completeness of PRM* is implied by its asymptotic optimality, proved in Section 4.2. 

Theorem 22 (Completeness of PRM*) The PRM* algorithm is probabilistically complete. 

Probabilistic completeness of RRG and RRT* is a straightforward consequence of the proba- 
bilistic completeness of RRT: 

Theorem 23 (Probabilistic completeness of RRG and RRT*) The RRG and RRT" algo- 
rithms are probabilistically complete. Furthermore, for any robustly feasible path planning problem 
(Affree, Xinit, 'Ygoal)? there cxist constants a > and no G N, both dependent only on X^ree (^f^d '^goab 
such that 

P({V;f^GnA'go,,7^0}) >l-e-«-, Vn>no, 

and 

P n A-go,! ^ 0}) > 1 - e-«", Vn > no. 

Proof By construction, V^^^{oo) = V^^^ (w) = V^^^{u), for all w G O and n G N. Moreover, 
the RRG and RRT* algorithms return connected graphs. Hence the result follows directly from 
the probabilistic completeness of RRT. □ 

In particular, note that if the RRT algorithm returns a feasible solution by iteration n, so will 
the RRG and RRT* algorithms, assuming the same sample sequence. 
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4.2 Asymptotic Optimality 

In this section, the optimality problem of path planning is considered. The algorithms presented 

in Section 3 arc analyzed, in terms of their ability to return solutions whose cost converge to the 
global optimum. First, a definition of asymptotic optimality is provided as almost-sure convergence 
to optimal paths. Second, it is shown that the RRT algorithm lacks the asymptotic optimality 
property. Third, the PRM*, RRG, and RRT* algorithms, as well as their fc-nearest implementations, 
are shown to be asymptotically optimal. 

Recall from Section 4.1 that an algorithm is probabilistically complete if the algorithm finds 
with high probability a solution to path planning problems that are robustly feasible, i.e., for 
which feasible path exists with strong (5-clearance. A similar approach is used to define asymptotic 
optimality, relying on a notion of weak (5-clearance and on a continuity property for the cost of 
paths, which will be introduced below. 

Let iTi,c72 G Sfree be two collision- free paths with the same end points. A path ai is said to 
be homotopic to (T2, if there exists a continuous function : [0, 1] — >■ Sfree, called the homotopy, 
such that ■0(0) = o'l, V'(l) = ^^2, and iP{t) is a collision-free path in for all r € [0, 1]. Intuitively, 
a path that is homotopic to a can be continuously transformed to a through Xf^cc (see Munkres, 
2000). A collision-free path a : [0,s] — )• Affree is said to have weak 5-clearance, if there exists a 
path a' that has strong (5-clearance and there exist a homotopy ■0, with = a, ■0(1) = a' , and 
for all a G (0, 1] there exists (5q, > such that ip{a) has strong (J^-clearance. See Figure 2 for an 
illustration of the weak (5-clearance property. A path that violates the weak (5-clearance property is 
shown in Figure 3. Weak (5-clearance does not require points along a path to be at least a distance 5 
away from the obstacles (see Figure 4). In fact, a collision-free path with uncountably many points 
lying on the boundary of an obstacle can still have weak (5-clearance. 




Figure 2: An illustration of a path a with weak (5-clearance. The path a' that lies inside 'm.ts{Xi^ce) 
and is in the same homotopy class as a is also shown in the figure. Note that a does not have 
strong (5-clearance. 




Figure 3: An illustration of an example path a that does not have weak (5-clearance. For any 
positive value of (5, there is no path in \nts{Xi^ce) that is in the same homotopy class as a. 

Next, the set of all paths with bounded length is introduced as a normcd space, which allows 
taking the limit of a sequence of paths. Recall that S is the set of all paths, and TV{-) denotes the 
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Front view 



Side view 



Figure 4: An illustration of a path that has weak (5-clearance. The path passes through a point 
where two spheres representing the obstacle region are in contact. Clearly, the path does not have 
strong 5-clearance. 

total variation, i.e., the length, of a path (see Section 2.1). Given ai,a2 G S with ai : [0,1] — )• X 
and (72 : [0,1] — >■ X, the addition operation is defined as {ai + (72) (r) = (Ti(r) + (72 (r) for all 
T G [0, 1]. The set of paths S is closed under addition. Given a path a : [0, 1] — )■ A" and a scalar 
a G M, the multiplication by a scalar operation is defined as {aa){T) := aa^r) for all r G [0, 1]. 
With these addition and multiplication by a scalar operations, the function space S is, in fact, a 
vector space. On the vector space S, define the norm ||(7||bv '■= Jq |c(''")| + TV((7), and denote 
the function space S endowed with the norm || • ||bv by BV(A'). The norm || • ||bv induces the 
following distance function: 



where || • || is the usual Euclidean norm. A sequence {(7„}„gN of paths is said to converge to a path 
a, denoted as lim„^oo o"n = o', if the norm of the difference between an and a converges to zero, 
i.e., lim„^oo \Wn - ^Wby = 0. 

A feasible path a* G Affree that solves the optimality problem (Problem 3) is said to be a robustly 
optimal solution if it has weak (5-clearance and, for any sequence of collision-free paths {(7„}„gp!j, 
C7„ G Affree, Vn G N, such that lim„^oo<7n = cr*^ hnin^oo c((7h) = c((7*). Clearly, a path planning 
problem that has a robustly optimal solution is necessarily robustly feasible. Let c* = c{a*) be the 
cost of an optimal path, and let Y^^^ be the extended random variable corresponding to the cost 
of the minimum-cost solution included in the graph returned by ALG at the end of iteration n. 

Definition 24 (Asymptotic Optimality) An algorithm ALG is asymptotically optimal if, for 
any path planning problem ('^frce; -^init) A'goai) and cost function c : T, M>o that admit a robustly 
optimal solution with finite cost c* , 



Note that, since Y^^^ > c*, Vn G N, asymptotic optimality of ALG implies that the limit 
lim^^oo Y^^^ exists, and is equal to c*. Clearly, probabilistic completeness is necessary for asymp- 
totic optimality. Moreover, the probability that a sampling-based algorithm converges to an optimal 
solution almost surely has probability either zero or one. That is, a sampling-based algorithm either 
converges to the optimal solution in almost all runs, or the convergence does not occur in almost 
all runs. 

Lemma 25 Given that lim sup„^(^ Y^'^*^ < 00, i.e., ALG finds a feasible solution eventually, the 
probability that lim sup^^g^ Y^^^ = c* is either zero or one. 



dist((7l,(72) = ||(7i - (72||bV = 



Jo 
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Proof Conditioning on the event {limsup„_^j^ Y^'^^ < 00} ensures that Y^^'^ is finite, thus a 
random variable, for all large n. Given a sequence {l^jneN of random variables, let J^'^ denote 
the (7-field generated by the sequence {l^jj^^ of random variables. The tail a-field T is defined 
as T = rineN-^n- An event A is said to be a tail event if ^ G T. Any tail event occurs with 
probability either zero or one by the Kolmogorov zero-one law (Resnick, 1999). Consider the se- 
quence {Y^^'^}n<^n of random variables. Let J^'^^^ denote the cr-fields generated by {yn^'~^}^=m- 
Then, {limsup„^^yALG ^ {limsup„^^_„>^ F^^^^ = c*} G for ah n G N. Hence, 

{y^^*^ = c*"\ G HneN-^m is a tail event. The result follows by the Kolmogorov zero-one law. 
□ 

Among the first steps in assessing the asymptotic optimality properties of an algorithm ALG 
is determining whether the limit lim„_).oo Yn^^ exists. It turns out that if the graphs returned by 
ALG satisfy a monotonicity property, then the limit exists, and is in general a random variable, 
indicated with Y^^'^ . 

Lemma 26 IfGf^^{u) C Gf^^{u), Woo en and Vi G N, then lim„_^oo Y^^^{uj) = Y^'^^iu). 

Proof Since GfLG(^) c Gf'^^ioj), then ^^^^^(0;) < Y/'^^iu), for ah a; G O. Since y^^^G > ^* ^ 
then the sequence converges to some limiting value, dependent on oj, i.e., Y^^^{oj). □ 

Of the algorithms presented in Section 3, it is easy to check that PRM, sPRM, RRT, RRG, and 
RRT* satisfy the monotonicity property in Lemma 26. On the other hand, A;-nearest sPRM and 
PRM* do not: in these cases, the random variable l^+i*^ is not necessarily dominated by Y/^^^. 
This is evident in numerical experiments, e.g., see Figures 10 and 11 in Section 5. 

In order to avoid trivial cases of asymptotic optimality, it is necessary to rule out problems in 
which optimal solutions can be computed after a finite number of samples. Let S* denote the set 
of all optimal paths, i.e., the set of all paths that solve the optimal planning problem (Problem 3), 
and Xopt denote the set of states that an optimal path in S* passes through, i.e., 

^opt = {x e Xfree | 3(7* G S*, T G [0, 1] such that X = (7*(r)}. 

Assumption 27 (Zero-measure Optimal Paths) The set of all points traversed by an optimal 
trajectory has measure zero, i.e., ^ (^opt) = 0. 

Most cost functions and problem instances of interest satisfy this assumption, including, e.g., the 
Euclidean length of the path when the goal region is convex. This assumption does not imply that 
there is a single optimal path; indeed, there are problem instances with uncountably many optimal 
paths, for which Assumption 27 holds. (A simple example is the motion planning problem in three 
dimensional Euclidean space where a ball shaped obstacle is placed between the initial state and 
the goal region.) Assumption 27 implies that no sampling-based planning algorithm can find a 
solution to the optimality problem in a finite number of iterations. 

Lemma 28 // Assumption 27 holds, the probability that a sampling-based algorithm ALG returns 
a graph containing an optimal path at a finite iteration n G N is zero, i.e., 

P (u„eN{y„ALG = c*}) = 0. 

Proof Let Bn denote the event that ALG constructs a graph containing a path with cost exactly 
equal to c* at the end of iteration i, i.e., Bn = {1^^^ = c*}. Let B denote the event that ALG 
returns a graph containing a path that costs exactly c* at some finite iteration i. Then, B can be 
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written as B = U„gM^n- Since Bn ^ Bn+i, by monotonocity of measures, linij^oo 1P(-B„) = ¥(B). 
By Assumption 27 and tlic definition of the sampling procedure, ¥{Bn) = for all n € N, since the 
probability that the set (J"^^{SampleFree(i)} of points contains a point from a zero-measure set 
is zero. Hence, P(S) = 0. □ 

In the remainder of the paper, it will be tacitly assumed that Assumption 27, and hence Lemma 
28, hold. 

4.2.1 Existing algorithms 

The algorithms in Section 3.2 were originally introduced to efficiently solve the feasibility problem, 

relaxing the completeness requirement to probabilistic completeness. Nevertheless, it is of interest to 
establish whether these algorithms are asymptotically optimal in addition to being probabilistically 
complete. (The first two results in this section rely on results that will be proven in Section 4.2.2, 
i.e., the fact that the RRT algorithm is not asymptotically optimal, and the PRM* algorithm is 
asymptotically optimal) 

First, consider the PRM algorithm and its variants. The PRM algorithm, in its original form, 
is not asymptotically optimal. 

Theorem 29 (Non-optimality of PRM) The PRM algorithm is not asymptotically optimal. 

Proof The proof is based on a counterexample, establishing a form of equivalence between PRM 
and RRT, which in turn will be proven not to be asymptotically optimal in Theorem 33. Consider 
a convex obstacle-free environment, e.g., Xf^ee = X, and choose the connection radius for PRM and 
the steering parameter for RRT such that r,r] > diam(Af). At each iteration, exactly one vertex 
and one edge is added to the graph, since (i) all connection attempts using the local planner (e.g., 
straight line connections as considered in this paper) are collision-free, and (ii) at the end of each 
iteration, the graph is connected (i.e., it contains only one connected component). In particular, 
the graph returned by the PRM algorithm in this case is a tree, and the arborescence obtained by 
choosing as the root the first sample point, i.e., SampleFreeQ, is an online nearest-neighbor graph 
(see Section 2.2) coinciding with the graph returned by RRT with the random initial condition 
a^init = SampleFreeQ. 

Recall that the PRM algorithm is applicable for multiple-query planning problems: in other 
words, the graph returned by the PRM algorithm is used to solve path planning problems from 
arbitrary Xinit € -^free and -Ygoai C -Yfree- (Note that all such problems admit robust optimal 
solutions.) In particular, for Xinit = SampleFreeQ, and any Xgoai, then Y^^^{uj) = y^^'^(tj), for 
all a; G ri, n G N. In particular, since both PRM and RRT satisfy the monotonicity condition in 
Lemma 26, Theorem 33 implies that 

□ 

The lack of asymptotic optimality of PRM is due to its incremental construction, coupled with 
the constraint eliminating edges making unnecessary connections within a connected component. 
Such a constraint is not present in the batch construction of the sPRM algorithm, which is indeed 
asymptotically optimal (at the expense of computational complexity, see Section 4.3). 
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Theorem 30 (Asymptotic Optimality of sPRM) The sPRM algorithm is asymptotically op- 
timal. 

Proof By construction, V;^^^^^^) ^ yPRM*^^)^ Ef^^{oj) D E^^^* {oj) for all u & ^l. 
Hence, the graph returned by sPRM includes all the paths that are present in the graph returned 
by PRM* . Then, asymptotic optimality of sPRM follows from that of PRM* , which will be proven 
in Theorem 34. □ 



On the other hand, as in the case of probabilistic completeness, the heuristics that are often 
used in the practical implementation of (s)PRM are not asymptotically optimal. 

Theorem 31 (Non-optimality of /^-nearest sPRM) The k-nearest sPRM algorithm is not asymp- 
totically optimal, for any constant G N. 

This theorem will be proven under the assumption that the underlying point process is Poisson. 
More precisely, the algorithm is analyzed when it is run with Poisson(n) samples. That is, the real- 
ization of the random variable Poisson(n) determines the number of points sampled independently 
and uniformly in -Yfree- Hence, the expected number of samples is equal to n, although its real- 
ization may slightly differ. However, since the Poisson random variable has exponentially-decaying 
tails, its large deviations from its mean is unlikely (see, e.g., Grimmett and Stirzaker (2001) for a 
more precise statement). With a slight abuse of notation, the cost of the best path in the graph 
returned by the A;-nearest sPRM algorithm when the algorithm is run with Poisson(n) number of 
samples is denoted by Y^^^^, and it is shown that P({limsup„_^j^ Y^^^^ = c*}) = 0. 

Proof of Theorem 31 Let a* denote an optimal path and s* denote its length, i.e., s* = TV{a*). 
For each n, consider a tiling of a* with disjoint open hypercubes, each with edge length 2n~^/'^, 
such that the center of each cube is a point on cr*. See Figure 5. Let M„ denote the maximum 
number of tiles that can be generated in this manner and note Mn > ^ n^^'^. Partition each tile 
into several open cubes as follows: place an inner cube with edge length n~^^'^ at the center of the 
tile and place several outer cubes each with edge length ^ n~^/'^ around the cube at the center as 
shown in Figure 5. Let denote the number of outer cubes. The volumes of the inner cube and 
each of the outer cubes are and 2~'^n~^, respectively. 




Figure 5: An illustration of the tiles mention in the proof of Theorem 31. A single tile is shown in 
the left; a tiling of the optimal trajectory a* is shown on the right. 

For n G N and m G {1, 2, . . . , M„}, consider the tile m when the algorithm is run with Poisson(n) 
samples. Let In,m denote the indicator random variable for the event that the center cube of this 
tile contains no samples, whereas every outer cube contains at least k -\- 1 samples, in tile m. 

The probability that the inner cube contains no samples is e^^^ The probability that 
an outer cube contains at least A; -|- 1 samples is 1 — P ({Poisson(2~'^//x(A'free)) > A; -|- 1}) = 1 — 
P({Poisson(2-'^//x(-Yfree)) < A;}) = 1 - r(fc+i,2-^/^(A'f,ee)) ^ ^^lere T{-,-) is the incomplete gamma 
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function (Abramowitz and Stegun, 1964). Then, noting that the cubes in a given tile are disjoint 
and using the independence property of the Poisson process (see Lemma 11), 



E [In, 



-l//t(A'free) 



1 



r(fe + l,2-VM(A'free)) 



> 0, 



which is a constant that is independent of n; denote this constant by a. 

Let Gn = {Vn,En) denotc the graph returned by the A;-nearest PRM algorithm by the end of 
Poisson(n) iterations. Observe that if In,m = 1, then there is no edge of Gn crossing the cube of 
side length ^ n~^^'^ that is centered at the center of the inner cube in tile m (shown as the white 
cube in Figure 6). To prove this claim, note the following two facts. First, no point that is outside 
of the cubes can have an edge that crosses the inner cube. Second, no point in one of the outer 



cubes has an edge that has length greater than ^ ; 
illustrated in Figure 6. 



-l/d 



Thus, no edge can cross the white cube 



Figure 6: The event that the inner cube contains no points and each outer cube contains at least 
k points of the point process is illustrated. The cube of side length \ rC^I'^ is shown in white. 

Let Gn denote the path in Gn that is closest to a* in terms of the bounded variation norm. Let 



■■= \Wn - t^*||BV- Notice that Un > \ tT^I'^ Em=l ^ 



n 



-^/^Mnln,l = Tln,l. Then, 



E 



lim sup Un 

n—^oo 



> limsupE[C/n] > lim sup — E [7„,^] > 



a s 



> 0, 



where the first inequality follows from Fatou's lemma (Resnick, 1999). This implies P({lim sup„_^(^ Un > 
0}) > 0. Since U > imphes F„ > c* surely, 

P ({limsup„_,^ Yn>c*})>F ({limsup„_,^ Un > 0}) > 0. 

That is, P ({limsup„_^t^ Yn = c*}) < 1. In fact, by Lemma 25, P ({limsup„_^(^ Yn = c*}) = 0. □ 

Second, asymptotic optimality of a large class of variable radius sPRM algorithms is considered. 
Consider a variable radius sPRM in which connection radius satisfies r(n) < ^n"^/'^ for some 7 > 
and for all n G N. The next theorem shows that this algorithm lacks the asymptotic optimality 
property. 

Theorem 32 (Non-optimality of variable radius sPRM with r{n) = jn~^^'^) Consider a vari- 
able radius sPRM algorithm with connection radius r{n) = jn~^/'^. This sPRM algorithm is not 
asymptotic optimal for any 7 G M>o . 

Proof Let a* denote a path that is a robust solution to the optimality problem. Let n de- 
note the number of samples that the algorithm is run with. For all n, construct a set Bn = 



{Bn,i, Bn,2, ■ ■ ■ , Bn,M„} of Openly disjoint balls as follows. Each ball in Bn has radius rn 



7n 



-l/d 
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and lies entirely inside Affi-cc- Furthermore, the balls in Bn "tile" a* such that the center of each 
ball lies on a* (see Figure 7). Let M„ denote the maximum number of balls, s denote the length of 
the portion of a* that lies within the 5-interior of Affree, and no G N denote the number for which 
rn < S for all n > no- 
Then, for all n > no, 

Mn> 
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Figure 7: An illustration of the covering of the optimal path, a*, with openly disjoint balls. The 
balls cover only a portion of a* that lies within the 5-interior of Afrcc- 

Indicate the graph returned by this sPRM algorithm as Gn = {Vn,En). Denote the event that 
the ball -Bn,m contains no vertex in Vn by An^m- Denote the indicator random variable for the event 
■An,m by In,m, i-G-, In,m = 1 when An^m holds and In,m = otherwise. Then, for all n > no, 



E[/n,m] = P(^n,m) 



free ) 



1 



1 



/W(^frcc) n 



Let Nn be the random variable that denotes the total number of balls in that contain no 
vertex in Vn, i.e., Nn = Z]m=i ^ri,m,- Then, for all n > no, 



m=l 



Consider a ball Bn,m that contains no vertices of this sPRM algorithm. Then, no edges of the 
graph returned by this algorithm cross the ball of radius ^r„ centered at the center of Sn,m- See 
Figure 8. 

Let Pn denote the (finite) set of all acyclic paths that reach the goal region in the graph returned 
by this sPRM algorithm when the algorithm is run with n samples. Let Un denote the total variation 
of the path that is closest to a* among all paths in P„, i.e., Un := min^-^gp^ ||(T„ — (T*||bv- Then, 



E[Un] > E 



1/d 



Nn 



> 



1 - 



2 V /(/(-Yfree) n 

Taking the limit superior of both sides, the following inequality can be established: 

CdT 1 



E 



lim sup Un 



> lim sup E [Un] > hm sup - I 1 



- e ''(•^free) > 0, 



where the first inequality follows from Fatou's lemma (Rcsnick, 1999). Hence, P({limsup„^oQ f/„ > 
0}) > 0, which implies that F ({limsup^^o^ Y^^'^ > c*}) > 0. That is, P ({limsup^^^ Y^^'^ = c*}) < 
1. In fact, P ({limsup„_^j^ = c*}) = by the Kolmogorov zero-one law (see Lemma 25). □ 
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Figure 8: If the outer ball does not contain vertices of the PRM graph, then no edge of the graph 
corresponds to a path crossing the inner ball. 

Rapidly-exploring Random Trees In this section, it is shown that the minimum-cost path in 

the RRT algorithm converges to a certain random variable, however, under mild technical assump- 
tions, this random variable is not equal to the optimal cost, with probability one. 

Theorem 33 (Non-optimality of RRT) The RRT algorithm is not asymptotically optimal. 

The proof of this theorem can be found in Appendix B. Note that, since at each iteration the RRT 

algorithm either adds a vertex and an edge, or leaves the graph unchanged, Gf^^"'"(a;) C Gf^^{Lii), 
for alH € N and all w G il, and hence the limit lim„^oo ^^^^ exists and is equal to the random 
variable F^^^^. In conjunction with Lemma 25, Theorem 33 implies that this limit is strictly 
greater than c* almost surely, i.e., P ({lim^^oo ^^^""^ > c*}) =1. In other words, the cost of the 
best solution returned by RRT converges to a suboptimal value, with probability one. In fact, it is 
possible to construct problem instances such that the probability that the first solution returned by 
the RRT algorithm has arbitrarily high cost is bounded away from zero (Nechushtan et al., 2010). 

Since the cost of the best path returned by the RRT algorithm converges to a random variable. 
Theorem 33 provides new insight explaining the effectiveness of approaches as in Ferguson and 
Stcntz (2006). In fact, running multiple instances of the RRT algorithm amounts to drawing 
multiple samples of i^J^^"^. 

4.2.2 Proposed algorithms 

In this section, the proposed algorithms are analyzed for asymptotic optimality, i.e., almost sure 
convergence to optimal solutions. It is shown that the PRM*, RRG, and RRT* algorithms, as well 
as their fe-nearest implementations, are all asymptotically optimal. The proofs of the following 
theorems are quite lengthy, and will be provided in the appendix. 

Recall that d denotes the dimensionality of the configuration space, iJ,{Xf^ee) denotes the Lebesgue 
measure of the obstacle-free space, and denotes the volume of the unit ball in the d-dimensional 
Euclidean space. Proofs of the following theorems can be found in Appendices C-G. 

Theorem 34 (Asymptotic optimality of PRM*) //7prm > 2(l + l/d)V'^ ^ MC^frcc) ^/'^^ ^f^^^ 
the PRM* algorithm is asymptotically optimal. 

Theorem 35 (Asymptotic optimality of /c-nearest PRM*) // fcpRM > e (1 -|- 1/d), then the 
k-nearest implementation of the PRM* algorithm is asymptotically optimal. 
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Theorem 36 (Asymptotic optimality of RRG) // 7prm > 2 (1 + l/df'^ \ Q ) ' ^^^^ 
the RRG algorithm is asymptotically optimal. 

Theorem 37 (Asymptotic optimality of fc-nearest RRG) // fcRRc > e(l + 1/d), then the 
k -nearest implementation of the RRG algorithm is asymptotically optimal. 

Theorem 38 (Asymptotic optimality of RRT*) //trrt* > (2 (l+l/d))^'^ { ^'^^cT^ Y^'^ ' 
the RRT* algorithm is asymptotically optimal. 

Theorem 39 (Asymptotic optimality of A;-nearest RRT*) If kjiRT* > 2*^+^ e (1 + 1/d), then 
the k-nearest implementation of the RRT algorithm is asymptotically optimal. 

The proof of the latter theorem follows from those of Theorems 37 and 38. 
4.3 Computational Complexity 

The objective of this section is to compare the computational complexity of the algorithms provided 
in Section 3. First, each algorithm is analyzed in terms of the number of calls to the CollisionFree 
procedure. Second, the computational complexity of certain primitive procedures such as Nearest 
and Near (see Section 3.1) are analyzed. Using these results, a thorough analysis of the computa- 
tional complexity of the all the algorithms is given in terms of the number of simple operations, 
such as comparisons, additions, multiplications. An analysis of the computational complexity of 
the query phase, i.e., the complexity of extracting the optimal solution from the graph returned by 
these algorithms, is also provided. 

The following notation for asymptotic computational complexity will be used throughout this 
section. Let W^^^{P) be a function of the graph returned by algorithm ALG when ALG is run 
with inputs P = (-^free; a^init; -^goai) ^nd u. Clearly, W^^'^{P) is a random variable. Let / : N — >■ N 
be an increasing function with lim„^oo /(^) = oo. The random variable W^^^ is said belong to 
r2(/(n)), denoted as W^^^'^ € fi{f{n)), if there exists a problem instance P = (^freo a^init, '^goal) 
such that liminf„^ooIE[M^n^'^(^')//(n)] > 0. Similarly, W^^G ^^^^ belong to 0(/(n)) if 
limsup„^(^E[W^^^(P)//(n)] < oo for all problem instances P = (-^free; s^mit; -^goaO- 

Number of calls to the CollisionFree procedure Let M,f^*^ denote the total number of 
calls to the CollisionFree procedure by algorithm ALG in iteration n. 
First, lower-bounds are established for the PRM and sPRM algorithms. 

Lemma 40 (PRM) M™^^ G 0(ra). 

Proof Consider the problem instance (^frco a:init, ^^goal)) where Xfj-cc is composed of two openly- 
disjoint sets Xi and X2 (see Figure 9). The set X2 is designed to be a hyperrectangle shaped set 
with one side equal to r/2, where r is the connection radius. 

Any r-ball centered at a point in X2 will certainly contain a nonzero measure part of X2. Define 
p. as the volume of the smallest region in X2 that can be intersected by an r-ball centered at X2, 
i.e., p, := mix&X2 K^^^r n Xi). Clearly, > 0. 

Thus, for any sample A„ that falls into A2, the PRM algorithm will attempt to connect Xn to a 
certain number of vertices that lies in a subset X[ of Xi such that IJ.{X[) > fi. The expected number 
of vertices in X[ is at least jl n. Moreover, none of these vertices can be in the same connected 
component with A„. Thus, K[M^^^/n] > Jl. The result is obtained by taking the limit inferior of 
both sides. □ 
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Figure 9: An illustration of Affree = Xi U X2. 



Lemma 41 (sPRM) Mf^^ e n{n). 

Proof The proof of a stronger result is provided. It is shown that for all problem instances 
P = (A'free,a::init,'^goai)5 liminf„_^oo ]E[M^^^''^/n] > 0, which implies the lemma. Recall from Al- 
gorithm 2 that r denotes the connection radius. Let fi denote the volume of the smallest re- 
gion that can be formed by intersecting Affree with an r-ball centered at a point inside Affree, i.e., 
fi := mfx£X[,.eg l^{Bx,r n Afree)- Recall that A^tee is the closure of an open set. Hence, p,> 0. 

Clearly, M„, the number of calls to the CollisionFree procedure in iteration n, is equal to the 
number of nodes inside the ball of radius r centered at the last sample point X„. Moreover, the 
volume of the ^free tti^t lies inside this ball is at least p,. Then, the expected value of Mn is lower 
bounded by the expected value of a binomial random variable with parameters /x///(Affree) and n, 
since the underlying point process is binomial. Thus, E[M^^^'^] > — j n. Then, E[M„/n] > 
P'/'^hee for all n G N. Taking the limit inferior of both sides gives the result. □ 

Clearly, for /c-nearest PRM, M^-'P^'^ = k for all n e N with n > k. Similarly, for the RRT, 
M^'^ = 1 for all n G N. 

The next lemma upper-bounds the number of calls to the CollisionFree procedure in the 
proposed algorithms. 

Lemma 42 (PRM*, RRG, and RRT*) M^^^* , M^^^, M^^^* e O(logn). 

Proof First, consider PRM*. Recall that r„ denotes the connection radius of the PRM* algorithm. 
Recall that the r„ interior of Xf^ee^ denoted by intr„(-^free)) is defined as the set of all points x, for 
which the r„-ball centered at x lies entirely inside Afrcc- Let A denote the event that the sample 
Xn drawn at the last iteration falls into the r„ interior of ^free- Then, 

E [M^^^*] = E [M^^^* I A] F{A) + E [M^^^* \ A'=] P(A^). 

Let no G N be the smallest number such that /x(intr„(Affree)) > 0. Clearly, such no exists, since 
lim„_^oo fn = ^ and Xf^ee has non-empty interior. Recall that Q is the volume of the unit ball 
in the c?-dimcnsional Euclidean space and that the connection radius of the PRM* algorithm is 
fn = 7PRM(logn/n)^/°'. Then, for all n > no 



e[mp 



PRM* 



Cd 7PRM 



/i(intr„(Affree)) 



logn. 



On the other hand, given that Xn ^ int,.„(Aficc), the r„-ball centered at Xn intersects a fragment 



of ^bcc that has volume less than the volume of an rr, 
Then, for all n > no, EIM™" \ A"] < E[M^^^' | A] . 



-ball in the d-dimensional Euclidean space. 
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Hence, for all n> no, 

E " 



logn 



^ Cd 7PRM ^ CdTPRM 



/x(inV„ (Affree)) Ai(intr„o (-^free)) ' 



Next, consider the RRG. Recall that 77 is the parameter provided in the Steer procedure (see 
Section 3.1). Let D denote the diameter of the set -Yfreej i-^-) ■= ^^Px,x'eXhee 11^ ~ ^'11- Clearly, 
whenever r] > D, V^^^ = V^^'^ = y^^"^ surely, and the claim holds. 

To prove the claim when r] < D, let Cn denote the event that for any point x G ^free the RRG 
algorithm has a vertex x' G V^^^ such that ||x — < 77. As shown in the proof of Theorem 36 
(see Lemma 63), there exists a,b> such that P(C^) < ae~^". Then, 

E [M^^^] = E [M^^^ I Cn] P(C„) + E [M,^^^ | C^] P(C7fJ, 

Clearly, E [M^^*^ | C^] < n. Hence, the second term of the sum on the right hand side converges 
to zero as n approaches infinity. On the other hand, given that C„ holds, the new vertex that 
will be added to the graph at iteration n, if such a vertex is added at all, will be the same as the 
last sample, X„. To complete the argument, given any set of n points placed inside yu(Xfj.cc), let 
Nn denote the number of points that are inside a ball of radius r„ that is centered at a point X„ 
sampled uniformly at random from /i(Xfree)- The expected number of points inside this ball is no 
more than ^ n. Hence, E[M^^^ \ Cn] < ^'(x^"'^ logn, which implies the existence of a constant 

^1 G M>o such that Mm sup^^o^ E[M^^G/(logn)] < (t)i. 

Finally, since M^^^* = M^^<^ holds surely, limsup„_^(^ E[M^^'^/(logn)] < ^1 also. □ 

Trivially, M^"^^^* = M^'^^^ = M^'^^^* =k\ogn for ah n with n/\ogn> k. 



Complexity of the CollisionFree procedure In this section, complexity of the CollisionFree 
procedure in terms of the number of obstacles in the environment is analyzed, which is a widely- 
studied problem in the literature (see, e.g., Lin and Manocha (2004) for a survey). The main result 
is based on Six and Wood (1982), which shows that checking collision with m obstacles can be 
executed in O(log'^m) time using data structures based on spatial trees (see also Edelsbrunner and 
Maurer, 1981; Hopcroft et al., 1983). 



Complexity of the Nearest procedure The nearest neighbor search problem has been widely 

studied in the literature, since it has many applications in, e.g., computer graphics, database 
systems, image processing, data mining, pattern recognition, etc. (Samet, 1989b, a). Clearly, a 
brute-force algorithm that examines every vertex runs in 0(n) time and requires 0(1) space. How- 
ever, in many online real-time applications such as robotics, it is highly desirable to reduce the 
computation time of each iteration under sublinear bounds, e.g., in O(logn) time, especially for 
anytime algorithms that provide better solutions as the number of iterations increase. 

Fortunately, existing algorithms for computing an "approximate" nearest neighbor, if not an 
exact one, are computationally very efficient. In the sequel, a vertex y is said to be an e-approximate 
nearest neighbor of a point ,Tif||y — a:|| < (l+e)||2; — where z is the true nearest neighbor of 
X. An approximate nearest neighbor can be computed using balanced-box decomposition (BBD) 
trees, which achieves 0{cd,e^ogn) query time using 0{dn) space (Arya et al., 1999), where Cd,e < 
d\\+Qd/e\'^. This algorithm is computationally optimal in fixed dimensions, since it closely matches 
a lower bound for algorithms that use a tree structure stored in roughly linear space (Arya et al., 
1999). Using approximate nearest neighbor computation in the context of both PRMs and RRTs 
was discussed very recently in Yershova and LaValle (2007); Plaku and Kavraki (2008). 
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Let G = {V,E) be a graph with V X and let x £ X. The discussion above imphes that 
the number of simple operations executed by the Nearest(G, x) procedure is 0(log|y|) in fixed 
dimensions, if the Nearest procedure is implemented using a tree structure that is stored in linear 
space. 

Complexity of the Near procedure Problems similar to that solved by the Near procedure are 
also widely-studied in the literature, generally under the name of range search problems, as they 
have many applications in, for instance, computer graphics and spatial database systems (Samet, 
1989a). In the worst case and in fixed dimensions, computing the exact set of vertices that reside 
in a ball of radius r,„ centered at a query point x takes 0{n^~^/'^ + m) time using k-d trees (Lee 
and Wong, 1977), where m is the number of vertices returned by the search (see also Chanzy et al. 
(2001) for an analysis of the average case). 

Similar to the nearest neighbor search, computing approximate solutions to the range search 
problem is computationally easier. A range search algorithm is said to be e-approximate if it returns 
all vertices that reside in the ball of size and no vertices outside a ball of radius (l+e) rn, but may 
or may not return the vertices that lie outside the former ball and inside the latter ball. Computing 
e-approximate solutions using BBD-trees requires 0(2''logn + (P{S-\/d/e)^~^) time when using 
0{dn) space, in the worst case (Arya and Mount, 2000). Thus, in fixed dimensions, the complexity 
of this algorithm is 0(logn + (1/e)'^^"'^), which is known to be optimal, closely matching a lower 
bound (Arya and Mount, 2000). More recently, algorithms that can provide trade-offs between 
time and space were also proposed (Arya et al., 2005). 

Note that the Near procedure can be implemented as an approximate range search while main- 
taining the asymptotic optimality guarantee. Notice that the expected number of vertices returned 
by the Near procedure also does not change, except by a constant factor. Hence, the Near proce- 
dure can be implemented to run in order log n expected time in the limit and linear space in fixed 
dimensions. 

Time complexity of the processing phase The following results characterize the asymptotic 
computational complexity of various sampling-based algorithms in terms of the number of simple 
operations such as comparisons, additions, and multiplications. 

Let n denote the total number of iterations (or, alternatively, the number of samples), and m 
denote the number of obstacles in the environment. Then, by Lemmas 40 and 41, N^^^, N^^^^'^ G 
r2(n^ log"^ m). In the fc-nearest sPRM and RRT algorithms, O(logn) time is spent on finding the 
(A;-)nearest ncighbor(s) and rt{log'^ m) time is spent on collision checking at each iteration. Hence, 

jyfc-sPRM^ jyRRT ^ ^og n + n log'^ m). 

In all the proposed algorithms, O(logn) time is spent on finding the near neighbors, and 
lognlog'^m time is spent on collision checking. Thus, N^^^ G 0(n lognlog'^ m) for ALG G 
{PRM*, A;-PRM*, RRG, A;-RRG, RRT*, A;-RRT*}. 

Time complexity of the query phase After algorithm ALG returns the graph G^^^, the op- 
timal path must be extracted from this graph using, e.g., Dijkstra's shortest path algorithm (Schri- 
jver, 2003). In this section, the complexity of this operation, called the query phase, is discussed. 

The following lemma yields the asymptotic computational complexity of computing shortest 
paths. Let G = (V, E) be a graph. A length function I : E ^ R>o is a function that assigns each 
edge in a positive length. Given a vertex v & V , the shortest paths tree for G, I, and u is a graph 
G' = {V,E'), where E' Q E such that for any v' £V \ {v}, there exists a unique path in G that 
starts from v and reaches v' , moreover, this path is the optimal such path in G. 
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Lemma 43 (Complexity of shortest paths (Schrijver, 2003)) Given a graph G = {V,E), a 
length function I : E ^ M>o, and a vertex v & V , the shortest path tree for G, I, and v can he found 
in time 0{\V\\og{\V\) + \E\). 

It remains to determine the number of vertices and edges in G^^*^ = {V^^'^ ,E^^'^), for each 
algorithm ALG. 

Trivially, |-E'^'"*^| G Vl{n) holds for all the algorithms discussed in this paper, in particular, 
for ALG G {PRM, fc-sPRM, RRT}. For the sPRM algorithm, a stronger bound can be provided: 
l^sPRMj g J7(n^). To prove this claim, consider the problem instance (^frco a^init, '^goai), where 
Affree = = (0, 1)"'. Then, the straight path between any two vertices will be collision-free. Thus, 
the number of edges is exactly equal to the number of calls to the CollisionFree procedure. Then, 
the result follows from Lemma 41. 

For the proposed algorithms, jE"^^^ |, jE"^^^] G 0(n log n). Since the number of edges is always 
less than or equal to the total number of calls to the CollisionFree procedure, this claim follows 
directly from Lemma 42. Finally \E^-^^^*\, \E^^^^\ e 0{n logn) and \E^'^^*\, \E!^-^^^* \ G 0(n) 
all hold trivially. 

Space complexity Space complexity of an algorithm ALG is defined as the amount of memory 
that is used by ALG to compute the graph G^^*^ = {V^^'^,E^^^). Clearly, in all algorithms 
discussed in this paper, the space complexity is the size of G^^^, i.e., jV^'"^! + l-E^'"*^!. Since the 
number of edges is at least as much as the number of vertices in Gf^^^^ for all algorithms discussed 
in this paper, the space complexity of an algorithm, in this context, is the number edges in the 
graph that it returns, which was determined in the previous section. 

5 Numerical Experiments 

This section is devoted to an experimental study of the algorithms considered in the paper. All 
algorithms were implemented in C and run on a computer with 2.66 GHz processor and 4GB RAM 
running the Linux operating system. Unless otherwise noted, total variation of a path is its cost. 

A first set of experiments were run to illustrate the different performance of /c-nearest PRM and 
of PRM* . The fc-nearest PRM and the PRM* algorithms were run alongside in two dimensional 
configuration-space and the cost of the best path in both algorithms is plotted versus the number of 
iterations in Figure 10. The fc-nearest PRM does not converge to optimal solutions, unlike PRM*. 
The performance of the PRM* algorithm is also shown in configuration spaces of dimensions up to 
five in Figure 11. 

The main bulk of the experiments were aimed at demonstrating the performance of the RRT* 
algorithm, especially in comparison with its "standard" counterpart, i.e., RRT. Three problem 

instances were considered. In the first two, the cost function is the Euclidean path length. 

The first scenario includes no obstacles. Both algorithms are run in a square environment. The 
trees maintained by the algorithms are shown in Figure 12 at several stages. The figure illustrates 
that, in this case, the RRT algorithm does not improve the feasible solution to converge to an 
optimum solution. On the other hand, running the RRT* algorithm further improves the paths in 
the tree to lower cost ones. The convergence properties of the two algorithms are also investigated 
in Monte-Carlo runs. Both algorithms were run for 20,000 iterations 500 times and the cost of the 
best path in the trees were averaged for each iteration. The results are shown in Figure 13, which 
shows that in the limit the RRT algorithm has cost very close to a \/2 factor the optimal solution 
(see LaValle and Kuffner (2009) for a similar result in a deterministic setting), whereas the RRT* 
converges to the optimal solution. Moreover, the variance over different RRT runs approaches 
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2.5, while that of the RRT* approaches zero. Hence, almost all RRT* runs have the property of 
convergence to an optimal solution, as expected. 

In the second scenario, both algorithms are run in an environment in presence of obstacles. 
In Figure 14, the trees maintained by the algorithms are shown after 20,000 iterations. The tree 
maintained by the RRT* algorithm is also shown in Figure 15 in different stages. It can be observed 
that the RRT* first rapidly explores the state space just like the RRT. Moreover, as the number 
of samples increase, the RRT* improves its tree to include paths with smaller cost and eventually 
discovers a path in a different homotopy class, which reduces the cost of reaching the target consid- 
erably. ResTilts of a Monte-Carlo study for this scenario is presented in Figure 16. Both algorithms 
were run alongside up until 20,000 iterations 500 times and cost of the best path in the trees were 
averaged for each iteration. The figures illustrate that all runs of the RRT* algorithm converges 
to the optimum, whereas the RRT algorithm is about 1.5 of the optimal solution on average. The 
high variance in solutions retTirncd by the RRT algorithm stems from the fact that there are two 
different homotopy classes of paths that reach the goal. If the RRT luckily converges to a path of 
the homotopy class that contains an optimum solution, then the resulting path is relatively closer 
to the optimum than it is on average. If, on the other hand, the RRT first explores a path of the 
second homotopy class, which is often the case for this particular scenario, then the solution that 
RRT converges to is generally around twice the optimum. 

Finally, in the third scenario, where no obstacles are present, the cost function is selected to 
be the line integral of a function, which evaluates to 2 in the high cost region, 1/2 in the low cost 
region, and 1 everywhere else. The tree maintained by the RRT* algorithm is shown after 20,000 
iterations in Figure 17. Notice that the tree either avoids the high cost region or crosses it quickly, 
and vice-versa for the low-cost region. (Incidentally, this behavior corresponds to the well known 
Snell-Descartes law for refraction of light, see Rowe and Alexander (2000) for a path-planning 
application.) 

To compare the running time, both algorithms were run alongside in an environment with no 
obstacles for up to one million iterations. Figure 18, shows the ratio of the running time of RRT* 
and that of RRT versus the number of iterations averaged over 50 runs. As expected from the 
complexity analysis of Section 4.3, this ratio converges to a constant value. A similar figure is 
produced for the second scenario and provided in Figure 19. 

The RRT* algorithm was also run in a 5-dimensional state space. The number of iterations 
versus the cost of the best path averaged over 100 trials is shown in Figure 20. A comparison 
with the RRT algorithm is provided in the same figure. The ratio of the running times of the 
RRT* and the RRT algorithms is provided in Figure 21. The same experiment is carried out for a 
10-dimensional configuration space. The results are shown in Figure 22. 

6 Conclusion 

This paper presented the results of a thorough analysis of sampling-based algorithms for optimal 
path planning. It is shown that broadly used algorithms from the literature, while probabilistically 
complete, are not asymptotically optimal, i.e., they will return a solution to the path planning 
problem with high probability if one exists, but the cost of the solution returned by the algorithm 
will not converge to the optimal cost as the number of samples increases. In particular, it is 
proven that the PRM and RRT algorithms are not asymptotically optimal. A simplified version 
of PRM is asymptotically optimal, but is computationally expensive. In addition, it is shown that 
certain heuristic versions of PRM are not only not asymptotically complete, but also not necessarily 
complete. 
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Figure 10: The cost of the best path in the A;-nearest sPRM algorithm, and that in the PRM* 
algorithm are shown versus the number of iterations in simulation examples with no obstacles. The 
/c-nearest sPRM algorithm was run for k = 5,7, 10, 13, 15, each of which is shown separately in 
blue, and the PRM* algorithm is shown in red. The values are normalized so that the cost of the 
optimal path is equal to one. The iterations were stopped when the query phase of the algorithms 
exceeded the memory limit (approximately 4GB). 



In order to address these limitations of existing algorithms, a number of new algorithms are 
introduced, and proven to be asymptotically optimal and computational efficient, with respect to 
probabilistically complete algorithms in this class. In other words, asymptotic optimality imposes 
only a constant factor increase in complexity with respect to probabilistic completeness. The first 
algorithm, called PRM*, is a variant of PRM, with a variable connection radius that scales as 
log(ra)/n, where n is the number of samples. In other words, the average number of connections 
made at each iteration is proportional to log(n). The second new algorithm, called RRG, incremen- 
tally builds a connected roadmap, augmenting the RRT algorithm with connections within a ball 
scaling as log(n)/n. The third new algorithm, called RRT*, is a version of RRG that incrementally 
builds a tree. Experimental evidence that demonstrate the effectiveness of the algorithms proposed 
and support the theoretical claims were also provided. 

A common theme in the paper is that, in order to ensure both asymptotic optimality and 
computational efficiency, connections between samples should be sought within balls of radius 
scaling as log(n)/n. If these balls shrink faster as n increases, the algorithms are not asymptotically 
optimal (but may still be probabilistically complete); on the other hand, if these balls shrink slower, 
the complexity of the algorithms will suff'er. On average, the proposed scaling laws will result in an 
average number of connections per iteration that is proportional to log(n). Hence, it is natural to 
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Figure 11: Cost of the best path in the PRM* algorithm is shown in up to 2, 3, 4, and 5 dimensional 
configuration spaces, in Figures (a), (b), (c), and (d), respectively. The initial condition and goal 
region are on opposite vertices of the unit cube (0, l)*^. The obstacle region is a cube centered at 
(0.5, 0.5, . . . , 0.5) and has volume 0.5 in all cases. 



consider variants of these algorithms that make connections to A;log(n) neighbors surely. Indeed, 
it is shown that these algorithms do share the same asymptotic optimality and computational 
efficiency properties of their counterparts, as long as k is no smaller than a constant ^rrq- It 
is remarkable that this constant only depends on the dimension of the space, and is otherwise 
independent from the problem instance. 

The analysis of the results in the paper relies on techniques used to analyze random geometric 
graphs. Indeed, the algorithms considered in this paper build graphs that have many characteristics 
in common with well known classes of random geometric graphs. Interestingly, such geometric 
graphs exhibit phase transition phenomena, including percolation and connectivity, for thresholds 
matching those found for probabilistic completeness and asymptotic optimality of sampling-based 
algorithms. This leads to a natural conjecture that a sampling-based path planning algorithm is 
probabilistically complete if and only if the underlying random geometric graph percolates, and is 
asymptotically optimal if and only if the underlying random geometric graph is connected. 
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Figure 12: A Comparison of the RRT* and RRT algorithms on a simulation example with no 
obstacles. Both algorithms were run with the same sample sequence. Consequently, in this case, 
the vertices of the trees at a given iteration number are the same for both of the algorithms; only 
the edges differ. The edges formed by the RRT algorithm are shown in (a)-(d) and (i), whereas 
those formed by the RRT* algorithm are shown in (e)-(h) and (j). The tree snapshots (a), (e) 
contain 250 vertices, (b), (f) 500 vertices, (c), (g) 2500 vertices, (d), (h) 10,000 vertices and (i), 
(j) 20,000 vertices. The goal regions are shown in magenta (in upper right). The best paths that 
reach the target in all the trees are highlighted ^Ith red. 
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Figure 13: The cost of the best paths in the RRT (shown in red) and the RRT* (shown in blue) 
plotted against iterations averaged over 500 trials in (a). The optimal cost is shown in black. The 
variance of the trials is shown in (b). 
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The work presented in this paper can be extended in numerous directions. First of all, it would 
be of interest to establish broader connections between sampling-based path planning algorithms 
and random geometric graphs, e.g., by proving or disproving the conjecture above, and by possibly 
improving on current algorithms through a better understanding of the underlying mathematical 
objects. Similar analysis techniques can also be used to analyze other sampling-based path planning 
algorithms that were not analyzed in this paper, such as EST. In addition, it is of interest to inves- 
tigate deterministic sampling-based algorithms, in which samples are generated using deterministic 
dense sequences of points with, e.g., low dispersion, as opposed to random sequences. 

Second, it is of great practical interest to address motion planning problems subject to more 
complex constraints. For example, motion planning problems for mobile robots should consider 
the robot's dynamics, and hence differential constraints on the feasible trajectories (these are also 
called kino-dynamic planning problems). In addition, it is of interest to consider optimal planning 
problems in the presence of temporal/logic constraints on the trajectories, e.g., expressed using 
formal specification languages such as Linear Temporal Logic, or the /x-calculus. Such constraints 
correspond to, e.g., rules of the road constraints for autonomous ground vehicles, mission spec- 
ifications for autonomous robots, and rules of engagement in military applications. Ultimately, 
incremental sampling-based algorithms with asymptotic optimality properties may provide the ba- 
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Figure 14: A Comparison of the RRT (shown in (a)) and RRT* (shown in (b)) algorithms on a 
simulation example with obstacles. Both algorithms were run with the same sample sequence for 
20,000 samples. The cost of best path in the RRT and the RRG were 21.02 and 14.51, respectively. 



sic elements for the on-line solution of differential games, as those arising when planning in the 
presence of dynamic obstacles. 

Finally, it is noted that the proposed algorithms may have applications outside of the robotic 
motion planning domain. In fact, the class of sampling-based algorithm described in this paper 
can be readily extended to deal with problems described by partial differential equations, such as 
the eikonal equation and the Hamilton-Jacobi-Bellman equation. 
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Appendix 
A Notation 

Let N denote the set of positive integers and M denote the set of reals. Let No = N U {0}, and 
M>o, K>o denote the sets of positive and non-negative reals, respectively. A sequence on a set A 
is a mapping from N to A, denoted as {ajjieN, where o,j € ^ is the clement that f € N is mapped 
to. Given a, 5 G M, closed and open intervals between a and b are denoted by [a,b] and (a, 6), 
respectively. The Euclidean norm is denoted by || • ||. Given a set C M*^, the closure of X is denoted 
by c\{X). The closed ball of radius r > centered at a; G W^, i.e., , i.e., {y £ M'^ | ||y — x|| < r}, 
is denoted as Bx.r', Bx^r is also called the r-ball centered at x. Given a set X C M'^, the Lebesgue 
measure of X is denoted by n{X)- The Lebesgue measure of a set is also referred to as its volume. 
The volume of the unit ball in W^, is denoted by Q, i-e., Cd = /"('Bo,i)- The letter e is used to denote 
the base of the natural logarithm, also called Euler's number. 

Given a probability space where $7 is a sample space, C 2^ is a (T— algebra, and 

P is a probability measure, an event A is an element of J^. The complement of an event A 
is denoted by A'^. Given a sequence of events {An}nGn, the event Ci'^^i Ai is denoted by 
lim sup„^(^ >1„ (also called the event that A^ occurs infinitely often); the event ^i^n 
denoted by liminf„^oo An. A (real) random variable is a measurable function that maps into R. 
An extended (real) random variable can also take the values ±00. The expected value of a random 
variable Y is E[Y] = JqY dF. A sequence of random variables {Yn}nm is said to converge surely 
to a random variable Y if lim„_^oo Yn{u;) = Y{uj) for all w G the sequence is said to converge 
almost surely if P({lim„^oo = Y}) = 1. Finally, if (p{uj) is a property that is cither true or 
false for a given a; G the event that denotes the set of all samples uj for which f{uj) holds, i.e., 

G O I <^(w) holds}, is written as {^}, e.g., {w G | lim^^oo Yn{u}) = Y{uj)} is simply written as 
{lim„_^oo^ = Y}. The Poisson random variable with parameter A is denoted by Poisson(A). The 
binomial random variable with parameters n and p is denoted by Binomial(n,p). 

Let f{n) and g(n) be two functions with domain and range N or M. The function /(n) is 
said to be 0{g{n)), denoted as f{n) G 0{g{n)), if there exists two constants M and no such that 
/(n) < Mg{n) for all n > uq. The function /(n) is said to be Q,{g{n)), denoted as /(n) G Q,{g{n)), 
if there exists constants M and no such that f{n) > Mg{ri) for all n > uq. The function f{n) is 
said to be <c){g{n)), denoted as /(n) G Q{g{n)), if f{n) G 0{g{n)) and f{n) G Q.{g{n)). 

Let be a subset of W^. A (directed) graph G = {V, E) on X is composed of a vertex set V 
and an edge set E, such that F is a finite subset of X, and E \s & subset oiV xV. A directed path 
on G is a sequence {vi,V2, ■ ■ ■ , Vn) of vertices such that (wj, Wj+i) G E for all 1 < i < n — 1. Given 
a vertex v & V, the sets {u G F | {u, v) G E} and {tt G F | {v, u) G E^ are said to be its incoming 
neighbors and outgoing neighbors, respectively. A (directed) tree is a directed graph, in which each 
vertex but one has a unique incoming neighbor; the vertex with no incoming neighbor is called the 
root vertex. Vertices of a tree are often also called nodes. 
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B Proof of Theorem 33 (Non-optimality of RRT) 

For simplicity, the theorem will be proven assuming that (i) the environment contains no obstacles, 
i.e., Xfj-ee = [0,1]'*, and (ii) the parameter r) of the steering procedure is set large enough, e.g., 
r/ > diam (Afrec) = Vd- One one hand, considering this case is enough to prove that the RRT 
algorithm is not asymptotically optimal, as it demonstrates a case for which the RRT algorithm 
fails to converge to an optimal solution, although the problem instance is clearly robustly optimal. 
On the other hand, these assumptions are not essential, and the claims extend to the more general 
case, but the technical details of the proof arc considerably more complicated. 

The proof can be outlined as follows. Order the vertices in the RRT according to the iteration 
at which they are added to the tree. The set of vertices that contains the A;-th child of the root 
along with all its descendants in the tree is called the A;-th branch of the tree. First, it is shown 
that a necessary condition for the asymptotic optimality of RRT is that infinitely many branches 
of the tree contain vertices outside a small ball centered at the initial condition. Then, the RRT 
algorithm is shown to violate this condition, with probability one. 

B.l A necessary condition 

First, we provide a necessary condition for the RRT algorithm to be asymptotically optimal. 

Lemma 44 Let < R < miy^x^^^^ \\y — XmitH- event {limjv^oo ^^^^ = c*} occurs only if the 
k-th branch of the RRT contains vertices outside the R-ball centered at Xinit for infinitely many k. 

Proof Let {xi,X2, ■ ■ ■} denote the set of children to the root vertex in the order they are added 
to the tree. Let T{xk) denote the optimal cost of a path starting from the root vertex, passing 
through Xk, and reaching the goal region. By our assumption that the measure of the set of all 
points that are on the optimal path is zero (see Assumption 27 and Lemma 28), the probability 
that T{xk) = c* is zero for all k Hence, 

oo 
k=l 

Let denote the event that at least one vertex in the k-th branch of the tree is outside the ball 
of radius R centered at Xjnit in some iteration of the RRT algorithm. Consider the case when the 
event {lim sup;;..^;,^ A^} does not occur and the events {T[xk) > c*} occur for all /e G N. Then, A^ 
occxurs for only finitely many k. Let K denote the largest number such that Ak occurs. Then, the 
cost of the best path in the tree is at least sup{r(a;fc) | G {1,2, . . . ,K}}, which is strictly larger 
than c*, since {T{xk) > c*} for all finite k. Thus, lim„_).oo ^^^^ > c* must hold. That is, we have 
argued that 

(limsupAfc)'n [(~]{r{xk) > c*}) C^lim^Y^^^ > c*}. 
Taking the complement of both sides and using monotonicity of probability measures, 

< p(Um.upA.)+p([J,^„{rfe) = c-}), 

where the last inequality follows from the union bound. The lemma follows from the fact that the 
last term in the right hand side is equal to zero as shown above. □ 
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B.2 Length of the first path in a branch 

The following result provides a useful characterization of the RRT structure. 

Lemma 45 Let U = {Xi,X2, . . . , Xn} be a set of independently sampled and uniformly distributed 
points in the d-dimensional unit cube, [0, l]'^. Let X^+i be a point that is sampled independently 
from all the other points according to the uniform distribution on [0, l]*^. Then, the probability that 

among all points in U the point Xi is the one that is closest to Xn+i is 1/n, for all i € {1, 2, . . . , n}. 
Moreover, the expected distance from X^+i to its nearest neighbor in U is n~^^'^. 

Proof Since the probability distribution is uniform, the probability that Xn+i is closest to Xi is 
the same for all i G {1, 2, . . . , n}, which implies that this probability is equal to 1/n. The expected 
distance to the closest point in U is an application of the order statistics of the uniform distribution. 
□ 

An immediate consequence of this result is that each vertex of the RRT has unbounded degree, 
almost surely, as the number of samples approaches infinity. 

One can also define a notion of infinite paths in the RRT, as follows. Let A be the set of infinite 
sequences of natural numbers a = (ai, 02, • . .)• For any i G N, let tTj : E — N*, (ai, a2, . . . , Oj, . . .) 
(«!, 02, . . . , ai), be a function returning the prefix of length i of an infinite sequence in A. The 
lexicographic ordering of A is such that, given a, (3 G T,, a < (3 if and only if there exists j G N such 
that ai = Pi for alH G N, z < j — 1, and aj < Pj. This is a total ordering of A, since N is a totally 
ordered set. Given a G A and i G N, let be the sum of the distances from the root vertex Xjnit 

to its ai-th child, from this vertex to its Q;2-th child, etc., for a total of i terms. Because of Lemma 
45, this construction is well defined, almost surely, for a sufficiently large number of samples. For 
any infinite sequence a G A, let = limj^+oo '^^^(a); the limit exists since C^i(a) is non-decreasing 
in i. 

Consider infinite strings of the form k = (fc, 1,1,...), A; G N, and introduce the shorthand 
:= -C^yt 1^1 , „). The following lemma shows that, for any k &N, has finite expectation, which 

immediately implies that takes only finite values with probability one. The lemma also provides 

a couple of other useful properties of jOk, which will be used later on. 

Lemma 46 The expected value K[jHk\ is non-negative and finite, and monotonically non-increasing, 
in the sense that E[>Ck+i] < lE[>Ck]7 for any A; G N. Moreover, \imk^^K[Ck\ = 0. 

Proof Under the simplifying assumptions that there are no obstacles in the unit cube and rj is 
large enough, the vertex set V^^^ of the graph maintained by the RRT algorithm is precisely the 
first n samples and each new sample is connected to its nearest neighbor in V^^"^. 

Define Zi as a random variable describing the contribution to £1 realized at iteration i; in other 
words, Zi is the distance of the i-th sample to its nearest neighbor among the first i — 1 samples if 
the i-th sample is on the path used in computing Ci, and zero otherwise. Then, using Lemma 45, 



E[jCi] = E 



.1=1 



£]E[Z,] = £]i-V<i i-^ = zeta(l + 1/d), 



i=l i=l 



where the second equality follows from the monotone convergence theorem and Zeta is the Riemann 
zeta function. Since Zeta(j/) is finite for any y > I, E[jCi] is a finite number for all d G N. 
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Let Nk be the iteration at which the first sample contributing to is generated. Then, an 
argument similar to the one given above yields 



oo Nk 
i=Nk+l i=l 

Then, clearly, E[Zlk+i] < lE[i2k] for all G N. Moreover, since > k, it is the case that 
limfe^ooE[£k] = 0. □ 

B.3 Length of the longest path in a branch 

Given G N, and the sequence k = (k, 1,1,.. .), the quantity siip^>k Ca is an upper bound on the 
length of any path in the A;-th branch of the RRT, or in any of the following branches. The next 
result bounds the probability that this quantity is very large. 

Lemma 47 For any e > 0, 




sup £q, > e > < 

a>k J y fc 

First, we state and prove the following intermediate result. 
Lemma 48 < ]E[£k], for all a > k. 

Proof The proof is by induction. Since a > k, then 7ri(a) > k, and Lemma 46 implies that 
E[i2(7ri(a),i,i,...)] ^ IE[/3k]- Moreover, it is also the case that, for any i e N (and some abuse of 
notation), E[>C(7ri+i{Q),i,i,...)] ^ l^['^(7rj(a:),i,i, ...)]) by a similar argument considering a tree rooted 
at the last vertex reached by the finite path Tri{a). Since (7ri+i(Q;), 1, 1, . . .) > (7ri(a), 1, 1, . . .) > 
{k, 1, 1, . . .), the result follows. □ 

Proof of Lemma 47 Define the random variable a := infja > k | > e}, and set a := k if 
Ca < e for all a > k. Note that a > k holds surely. Hence, by Lemma 48, E[>Ca] < E[>Ck]. Let /g 
be the indicator random variable for the event '■= {sup^^^Ca > e}- Then, 

where the last inequality follows from the fact that jOq, is at least e whenever the event occurs. 

□ 



A useful corollary of Lemmas 46 and 47 is the following. 
Corollary 49 For any e> 0, lim^^oo IF'({sup„>k -Cq, > e}) = 0. 

B.4 Violation of the necessary condition 

Recall from Lemma 44 that a necessary condition for asymptotic optimality is that the k-th branch 

of the RRT contains vertices outside the i?-ball centered at xinit for infinitely many k, where 
< i? < iniy^Xg^^i \\y ~ a^initll- Clearly, the latter event can occur only if longest path in the k-th 
branch of the RRT is longer than R for infinitely many k. That is, 

P ff lim yf^T ^ < p ( iimsup|sup„>k>Ca > R}] . 
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The event on the right hand side is monotonic in the sense that {sup^^j^^^ La > R} 5 {sup(^>]j Ca > 
R} for all A: € N. Hence, limfc^oo{supQ,>i^. Ca > R} exists. In particular, P(limsup^^oQ{supQ,>i^. Ca > 
R}) = P(limfc^oo{supQ>k'Ca > R}) = linik^^F({swp^^i^ Ca > R}), where the last equality follows 
from the continuity of probability measures. Since limjt-^oo IP ({supQ>k Ca > R}) = for all i? > 
by Corollary 49, P({lim„_,oo Y^^"^ = c*}) = 0. 

C Proof of Theorem 34 (Asymptotic optimality of PRM*) 

An outline of the proof is given below, before the details are provided. 
C.l Outline of the proof 

Let a* denote a robustly optimal path. By definition, a* has weak 5-clearance. First, define a 
sequence {^njneN such that 5n > for all n G N and 6n approaches zero as n approaches infinity. 
Construct a sequence {a^jneN of paths such that cr^ has strong (5„-clearance for all n G N and (T„ 
converges to a* as n approaches infinity. 

Second, define a sequence {^nlnsN- For all n G N, construct a set Bn = {Bn,i, -6^,2, • • • , Bn^Mn} 
of overlapping balls, each with radius qn, that collectively "cover" the path See Figures 23 
and 24. Let Xm G -Bn,m and Xm+i G ^n,m+i be any two points from two consecutive balls in 
Bn- Construct B^ such that (i) Xm and Xm+i have distance no more than the connection radius 
r(n) and (ii) the straight path connecting Xm and Xm+i lies entirely within the obstacle free space. 
These requirements can be satisfied by setting (5„ and Qn to certain constant fractions of r(n). 

Let An denote the event that each ball in Bn contains at least one vertex of the graph returned 
by the PRM* algorithm, when the algorithm is run with n samples. Third, show that An occurs 
for all large n, with probability one. Clearly, in this case, the PRM* algorithm will connect the 
vertices in consecutive balls with an edge, and any path formed in this way will be collision-free. 

Finally, show that any sequence of paths generated in this way converges to the optimal path 
a*. Using the robustness of a*, show that the cost of the best path in the graph returned by the 
PRM* algorithm converges to c(cr*) almost surely. 

C.2 Construction of the sequence {cr„}„gi^ of paths 

The following lemma establishes a connection between the notions of strong and weak (5-clearance. 

Lemma 50 Let a* be a path be a path that has strong S -clearance. Let {(JnjneN be a sequence of 
real numbers such that lim„_).oo Sn = and < 5„ < (5 for all n gN. Then, there exists a sequence 
{<7n}neN of paths such that lim„_^oo <^n = cr* (^n^d an has strong Sn-clearance for all n EN. 

Proof First, define a sequence {-^n}neN of subsets of .Yfree such that Xn is the closure of the 
(5„-interior of -Yfree, i.e., 

Xn := Cl(int5„(;tfi.ee)) 

for all n G N. Note that, by definition, (i) Xn are closed subsets of Xfree, and (ii) any point Xn has 
distance at least 6n to any point in the obstacle set X^hs- 

Then, construct the sequence {(T„}neN of paths, where an G as follows. Let V : [0, 1] 

Sfree denote the homotopy with ijj{0) = a*; the existence of ijj is guaranteed by weak (5-clearance of 
a*. Define 

an := max {a \ V'(a) G ^x„} and an := ip{an)- 
ae[o,i] 
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Since Tix„ is closed, the maximum in the definition of q;„ is attained. Moreover, since V'(l) 
strong (5-clearance and (5„ < 5, € ^x„, which implies the strong (5n-clearance of (T„. 

Clearly, UneN'^" ~ '^free; since lim^^oo = 0. Also, by weak 5-clearance of a*, for any a G 
(0, 1], there exists some Sa € (0, S] such that ijj{a) has strong (5a-clearance. Then, lim„_^oo CKn = 0, 
which implies lim„_^oo = c* ■ D 



Recall that the connection radius of the PRM* algorithm was defined as 



Tn = 7PRM 



(see Algorithm 4 and the definition of the Near procedure in Section 3.1). Let 9i be a small positive 
constant; the precise value of 6i will be provided shortly in the proof of Lemma 52. Define 



6n ■= min ^^^V n| , for all n € N. 



By definition, < Sn < S holds. Moreover, lim„_).oo Sn = 0, since lim„_^oo = 0. Then, by 
Lemma 50, there exists a sequence {crnjneN of paths such that lim„_^oo = cr* Sind an has strong 
(5„-clearance for all n G N. 

C.3 Construction of the sequence {Bnjne'N of sets of balls 

First, a construction of a finite set of balls that collectively "cover" a path cr^ is provided. The 
construction is illustrated in Figure 23. 

Definition 51 (Covering balls) Given a path an : [0, 1] — )■ X, and the real numbers qn, In £ ^^>0j 
the set CoveringBalls(o"n, qn, In) is defined as a set {Bn,i, Bn,2, ■ ■ ■ , Bn,M„} of Mn balls of radius 
qn such that Bn,m is centered at a(Tni), and 

• the center of Bn,i is a{0), i.e., ri = 0, 

• the centers of two consecutive balls are exactly In apart, i.e., Tm '■= min{r G [rm-i, 1] | ||(T(r) — 
o-(Tm-i)|| > In} for all me {2,3,.. . ,M„}, 

• and M —1 is the largest number of balls that can be generated in this manner while the center 
of the last ball, Bn,M„ is o'{l), i.e., tm„ = 1- 

For each n G N, define 



1 + 01 

Construct the set Bn = {Bn,i, Bn,2, ■ ■ ■ , Bn,Mn} of balls as 5„ := CoveringBalls((T„, ^i^rt) 
using Definition 51 (see Figure 23). By construction, each ball in Bn has radius qn and the centers 
of consecutive balls in Bn are Oiqn apart (sec Figure 24 for an illustration of covering balls with 
this set of parameters). The balls in Bn collectively cover the path a"„. 

C.4 The probability that each ball in Bn contains at lecist one vertex 

Recah that G^^^* = {V^^^* , E^^^^* ) denotes the graph returned by the PRM* algorithm, when 
the algorithm is run with n samples. Let ^n,m denote the event that the ball Bn,m contains at 
least one vertex of the graph generated by the PRM* algorithm, i.e., An,m = {Bn,m H V^f^^^ 7^ 0}. 
Let An denote the event that all balls in Bn contain at least one vertex of the PRM* graph, i.e., 

— I \m=l ^n,m- 
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Figure 23: An illustration of the CoveringBalls construction. A set of balls that collectively cover 
the trajectory (T„ is shown. All balls have the same radius, The spacing between the centers of 
two consecutive balls is L . 




Figure 24: An illustration of the covering balls for PRM* algorithm. The (5n-ball is guaranteed 
to be inside the obstacle-free space. The connection radius r„ is also shown as the radius of the 
connection ball centered at a vertex x G Bn,m- The vertex x is connected to all other vertices that 
lie within the connection ball. 



Lemma 52 //7prm > 2 (1 + ^^M^^^^'^^ then there exists a constant Oi > such that 

the event that every ball in contains at least one vertex of the PRM* graph occurs for all large 
enough n with probability one, i.e., 

P fliminf A„) = 1. 

\ n— >oo / 

Proof The proof is based on a Borel-Cantelli argument which can be summarized as follows. Recall 

that A'^ denotes the complement of An. First, the sum Yl'^=i^i-^n) shown to be bounded. By 
the Borel-Cantelli lemma (Grimmett and Stirzaker, 2001), this implies that the probability that 
An holds infinitely often as n approaches infinity is zero. Hence, the probability that An holds 
infinitely often is one. In the rest of the proof, an upper bound on P(A„) is computed, and this 
upper bound is shown to be summable. 

First, compute a bound on the number of balls in Bn as follows. Let s„ denote the length of 
an, i.e., Sn := TV(c7„). Recall that the balls in Bn were constructed such that the centers of two 
consecutive balls in Bn have distance 9i qn- The segment of cr„ that starts at the center of Bn,m 
and ends at the center of Bn,m+i has length at least OiQn, except for the last segment, which has 
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length less than or equal to 9iqn- Let no G N be the number such that Sn < S for all n > no- Then, 
for all n> no, 

Sn {l + 0l)Sn {2 + ei)Sn 



(2 + ei)sn ( n N '''^ 



card {Bn) =Mn < 



6'i7PRM Vogn^ 

Second, compute the volume of a single ball in Bi as follows. Recall that //(•) denotes the usual 
Lebesgue measure, and Q denotes the volume of a unit ball in the d-dimensional Euclidean space. 
For all n> no, 

in ^ /■ d ^ f Y /■ [ Y /- /TPRMVlogn 

For all n > I, the probability that a single ball, say Bn,i, does not contain a vertex of the graph 
generated by the PRM* algorithm, when the algorithm is run with n samples, is 



Cd f 7PRM \ log n 



//(Xfree) \2 + Oi J n J 

Using the inequality (1 — l/f{n)Y < e~'^/^^'^\ the right-hand side can be bounded as 

<d ( 7PRM Y^^rrr^ ( 7PRM 

¥{An,l) < e A-C^free) I 2+«l ) ^ =n f'(Xfree) \ ) . 



Hence, 



6'i7PRM VognJ 



6'i7PRM (logra)'^ 
where the first inequality follows from the union bound. 

Finally, En=i^P'(^n) < oo holds, if (iw) ~ 3 > 1' '^^^^^ ^e satisfied for any 

IPRM > 2(1 + (^"^Td^)^^'' appropriately choosing 9i. Then, by the Borel-Cantelh 

lemma (Grimmett and Stirzaker, 2001), P(limsup„_^j^ ^$^) = 0, which implies P(liminf„_>oo ^n) = 
1. □ 



C.5 Connecting the vertices in subsequent balls in S„ 

Let Zn ■= {xi,X2, . . . , XMn} be any set of points such that Xm G -Bn,m for each m G {1, 2, . . . , M„}. 
The following lemma states that for all n G N and all m G {1, 2, . . . , M„ — 1}, the distance between 

Xm and Xm+i is less than the connection radius, r„, which implies that the PRM* algorithm will 
attempt to connect the two points Xm and Xm+i if they are in the vertex set of the PRM* algorithm. 
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Lemma 53 If Xn,m G Bn,m and Xn,m+i € Bn,m+i, then ||xn,m+i - Xn,m\\ < Tn, for alln eN and 
dime {1,2, ...,Mi-l}. 

Proof Recall that each ball in S„ has radius Qn = jj^^- Given any two points Xm G Bn^m and 
Xm+i G Bn,m+i, Sill of the following hold: (i) Xm has distance to the center of Bn^„i, (ii) x„i^i 
has distance qn to the center of Bn^m+i, and (iii) centers of Bn,m and Bn,m+\ have distance 9\qn 
to each other. Then, 

1 -\- 6\ 

||3^n,m+l S^j^^Tfill < -\- 9\) Qri — Z , fn, 

I + ai 

where the first inequality is obtained by an application of the triangle inequality and the last 
inequality follows from the definition of (5„ = minjj, □ 

By Lemma 53, conclude that the PRM* algorithm will attempt to connect any two vertices in 
consecutive balls in B^- The next lemma shows that any such connection attempt will, in fact, be 
successful. That is, the path connecting Xn,m and Xn,m+i is collision-free for all m G {1, 2, . . . , M„}. 



Lemma 54 For alln and all m G {1, 2, . . . , M„}, if Xm G Bn,m and Xm+i G Bn,m+i, then the 
line segment connecting Xn,m and Xn,m+i ^*es in the obstacle-free space, i.e., 

aXn,m + (1 - a) Xn,m+1 G -'^free, for all a G [0, 1]. 

Proof Recall that an has strong (5„-delta clearance and that the radius g„ of each ball in i?„ was 
defined as g„ = jq?^, where 6*1 > is a constant. Hence, any point along the trajectory (T„ has 
distance at least (1 + ^i) qn to any point in the obstacle set. Let ym and Um+i denote the centers of 
the balls Bn,m and Bn^m+ii respectively. Since ym = o'{Tm) and ym+i = c('^m+i) for some and 
Tm+i, Vm and ym+1 also have distance (1 + Oi)qn to any point in the obstacle set. 
Clearly, \\xm ^ UmW ^ Qn- Moreover, the following inequality holds: 

Ikm+l-Z/mll < \\{Xm-ym+l) + {ym+l-ym)\\ < l^m+l-ym+l || + ||ym+l-ym|| < qn+Ol Qn = (l+^l) ^n- 

where the second inequality follows from the triangle inequality and the third inequality follows 
from the construction of balls in B^. 

For any convex combination Xa '■= otXm + (1 — a) Xm+i, where a G [0, 1], the distance between 
Xa and ym can be bounded as follows: 

\\{aXm + {)- + a)Xm+l) -ym\\ = ||q; (a^m - 2/m) + (1 + (a^m+l - 2/m)|| 

= a Ikm - Z/mll + (1 + a) lla^m+i - 2/m|| 
= aqn + {l+a){l + qn)<{l + ei)qn, 

where the second equality follows from the linearity of the norm. Hence, any point along the line 

segment connecting Xm and Xm+i has distance at most (1 + ^i) qn to ym- Since, ym has distance 
at least (1 + 9i)qn to any point in the obstacle set, the line segment connecting Xm and Xm+i is 
collision-free. □ 



58 



C.6 Convergence to the optimal path 

Let Pn denote the set of ah paths in the graph G^^* = {V^^^* , E^^^* ) . Let a'^ be the path 
that is closest to an in terms of the bounded variation norm among all those paths in P„, i.e., 
a'n := min^-'ep^ — (T„||. Note that the sequence {o"^}nGN is a random sequence of paths, since the 
graph GJj'^^^ , hence the set Pn of paths is random. The following lemma states that the bounded 
variation distance between cr^ and cr„ approaches to zero, with probability one. 

Lemma 55 The random variable \\a'n — <7„||bv converges to zero almost surely, i.e., 

P ({lim„_j.oo Wcr'n - o-nllsv = 0}) = 1. 

Proof The proof of this lemma is based on a Borel-Cantelli argument. It is shown that X^^gpj P(||<t^— 
Cnllev > e) is finite for any e > 0, which implies that ||cr^ — cr„|| converges to zero almost surely 
by the Borel-Cantelli lemma (Grimmctt and Stirzaker, 2001). This proof uses a Poissonization 
argument in one of the intermediate steps. That is, a particular result is shown to hold in the 
Poisson process described in Lemma IL Subsequently, the result is de-Poissonized, i.e., shown to 
hold also for the original process. 

Fix some e > 0. Let a, f3 £ (0, 1) be two constants, both independent of n. Recall that g„ is 
the radius of each ball in the set Bn of balls covering the path an- Let In,m denote the indicator 
variable for the event that the ball i?„,m has no point that is within a distance /3 qn from the center 
of Bn,m- For a more precise definition, let P Bn,m denote the ball that is centered at the center of 
Bn,m and has radius /3r„. Then, 




1, iii(5Bn,m)nV™' =0, 

0, otherwise. 



Let Kn denote the number of balls in Bn that do not contain a vertex that is within a /3qn distance 
to the center of that particular ball, i.e., Kn := J2m=i ^n,m- 

Consider the event that In,m holds for at most an a fraction of the balls in Bn, i.e., {Kn < a M„}. 
This event is important for the following reason. Recall that the vertices in subsequent balls in Bn 
are connected by edges in G^^^^ by Lemmas 53 and 54. If only at most an a fraction of the balls 
do not have a vertex that is less than a distance of P rn from their centers (hence, a (1 — a) fraction 
have at least one vertex within a distance of /5rn from their centers), i.e., {Kn < a Mn} holds, then 
the bounded variation difference between a'^ and cj„ is at most 

where L is a finite bound on the length of all paths in {cr„}„gN) i-^-, L := sup„gj^ TV(a"„). That is, 

{i^n < a Mn} C I lla; - (TnllBV < V2 (a + /3) l} 

Taking the complement of both sides and using the monotonicity of probability measures, 

P ({ lla; - (7„||bv > V2 (a + /3) l}) < P {{Kn > a Mn}) . 

In the rest of the proof, it is shown that the right hand side of the inequality above is summable 
for all small a,f3 > 0, which implies that P ({||(T^ — || > e}) is summable for all small e > 0. 

For this purpose, the process that provides independent uniform samples from Xf^ee is approx- 
imated by an equivalent Poisson process described in Section 2.2. A more precise definition is 
given as follows. Let {Xi, X2, ■ ■ . , Xn} denote the binomial point process corresponding to the 
SampleFree procedure. Let v < 1 he a constant independent of n. Recall that Poisson(z/n) 
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denotes the Poisson random variable with intensity i^n (hence, mean value z^n). Then, the pro- 
cess Vun '■= {-^1) ^2, • • • , -''^Poisson(i/n)} is a Poisson process restricted to ^{Xi^cc) with intensity 
vn/ ii{Xhee) (see Lemma 11). Thus, the expected number of points of this Poisson process is un. 

Clearly, the set of points generated by one process is a subset of the those generated by the 
other. However, since < 1, in most trials the Poisson point process 7^,^„ is a subset of the binomial 
point process. 

Define the random variable denote the number of balls of that fail to have one sample 
within a distance /9r„ to their centers, when the underlying point process is Vun (instead of the 
independent uniform samples provided by the SampleFree procedure). In other words, is the 
random variable that is defined similar to except that the former is defined with respect to the 
points of Vvn whereas the latter is defined with respect to the n samples returned by SampleFree 
procedure. 

Since {Kn > a Mn} is a decreasing event, i.e., the probability that it occurs increases if Vvn 
includes fewer samples, the following bound holds (see, e.g., Penrose, 2003) 

^{{Kn > aMn}) < f{{Kn > a Mn}) +P({Poisson(z/n) > n}). 

Since a Poisson random variable has exponentially-decaying tails, the second term on the right 
hand side can be bounded as 

P({Poisson(i/n) > n}) < e"'^", 

where c > is a constant. 

The first term on the right hand side can be computed directly as follows. First, for all small 
P, the balls of radius (3rn are all disjoint (see Figure 25). Denote this set of balls by Bn,m = 
{Bn,i, Bn,2, ■ ■ ■ ,Bn,Mn}- More precisely, Bn,m is the ball of radius /3g„ centered at the center of 
Bn^m- Second, observe that the event {Kn > aA-In} is equivalent to the event that at least an a 
fraction of all the balls in Bn include at least one point of the process V^n- Since, the point process 
Vun is Poisson and the balls in B^ are disjoint for all small enough (3, the probability that a single 
ball in Bn does not contain a sample is Pn ■= exp(— Crf {/3qn)'^ f n/ iJ,{X{j.ee)) < exp(— c/3z^ logn) for 
some constant c. Third, by the independence property of the Poisson point process, the number 
of balls in Bn that do not include a point of the point process Vun is a binomial random variable 
with parameters Af„ and Pn- Then, for all large n, 

f({K„ > aM„}) < P({Binomial(M„,p„) > aM^}) < exp(-M„^?„). 

Combining the two inequalities above, the following bound is obtained for the original sampling 
process 

^{{Kn > aMn}) < e-^^ + e-^"^'". 

Summing up both sides, 

oo 

"^FilKn > an}) < oo. 

n=l 

This argument holds for all a, /?, z/ > 0. Hence, for all e > 0, 

oo 

5^F({||c7;-an||BV >e}) <oo. 

n=l 

Then, by the Borel-Cantelli lemma, P ({lim^^oo \\(^n ~ '^uWby = 0}) = 1. □ 
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Figure 25: The set Bn,m of non-intersection balls is illustrated. 



Finally, the following lemma states that the cost of the minimum cost path in the graph returned 
by the PRM* algorithm converges to the optimal cost c* with probability one. Recall that Y^^^ 
denotes the cost of the minimum-cost path in the graph returned by the PRM* algorithm, when 
the algorithm is run with n samples. 

Lemma 56 Under the assumptions of Theorem 34, the cost of the minimum-cost path present in 
the graph returned by the PRM* algorithm converges to the optimal cost c* as the number of samples 
approaches infinity, with probability one, i.e.. 



Proof Recall that a* denotes the optimal path, and that lim^^oo = c* holds surely. By 
Lemma 55, lim„_>oo \Wn ~ o'nIlBV = holds with probability one. Thus, by repeated application of 
the triangle inequality, Yvnin^^ 

\Wn ~ ^*I|bv = 0, i.e.. 



That is the costs of the paths {u^jIneN converges to the optimal cost almost surely, as the number 



D Proof of Theorem 35 (Asymptotic Optimality of /c-nearest PRM*) 

The proof of this theorem is similar to that of Theorem 34. For the reader's convenience, a complete 
proof is provided at the expense of repeating some of the arguments. 





Then, by the robustness of the optimal path a*, it follows that 




of samples approaches infinity. 



□ 
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D.l Outline of the proof 



Let a* be a robust optimal path with weak (5-clearance. First, define the sequence {cr„}neN of paths 
as in the proof of Theorem 34. 

Second, define a sequence {qn}nm ^iid tils (Jn with a set i?„ = {Bn^i, Bn,2, ■ ■ ■ , Bn^u} of over- 
lapping balls of radius See Figures 23 and 26. Let Xm G -Bn,m and Xm+i G -Bn,m+i be any 
two points from subsequent balls in Construct B^ such that the straight path connecting Xm 
and .T„j+i lies entirely inside the obstacle free space. Also, construct a set B'^ of balls such that 
(i) -B^im and Bn,m, are centered at the same point and (ii) Bn,m contains Bn,mi and Bn,m+i^ for all 
mG{i,2,...,M„-l}. 

Let An denote the event that each ball in Bn contains at least one vertex, and A'^ denote the 
event that each ball in contains at most k{n) vertices of the graph returned by the A;-nearest 
PRM* algorithm. Third, show that An and occur together for all large n, with probability one. 
Clearly, this implies that the PRM* algorithm will connect vertices in subsequent ball in Bn with 
an edge, and any path formed by connecting such vertices will be collision-free. 

Finally, show that any sequence of paths formed in this way converges to a* . Using the ro- 
bustness of cj*, show that the best path in the graph returned by the fc-nearest PRM* algorithm 
converges to c((7*) almost surely. 

D.2 Construction of the sequence {an}n& of paths 

Let ^1, ^2 € M>o be two constants, the precise values of which will be provided shortly. Define 
:= min + ft) ((lii/^^^M^-l) (l^) "1 . 

Since lim„^oo = and < < 5 for all n € N, by Lemma 50, there exists a sequence 
{<7n}neN of paths such that lim„^oo <7n = <7* and (T„ is strongly (5„-clear for all n G N. 

D.3 Construction of the sequence {Bn}nm of sets of balls 

Define 

_ 5n 
1 + ^1 • 

For each n G N, use Definition 51 to construct a set Bn = Bn^2^ ■ ■ ■ , Bn^Mn) of overlapping 

balls that collectively cover an as Bn ■= CoveringBalls(c7„, ^ig^) (see Figures 23 and 26 for an 
illustration) . 



D.4 The probability that each ball in Bn contains at least one vertex 

Recall that Gf^'^* = {Vj^^^^* , EI^^^^* ) denotes the graph returned by the /c-nearcst PRM* 
algorithm, when the algorithm is run with n samples. Let An^m denote the event that the ball Bn.m 
contains at least one vertex from l^f^^^*, i.e., An^ni = {Bn,Tn n Vn^^^* ^ 0}. Let An denote the 
event that all balls in Bn,m contains at least one vertex of G^^^^* , i.e.. An = 0™=! ^n,m- 

Recall that denotes the complement of the event An, IJ,{-) denotes the Lebcsguc measure, 
and Q is the volume of the unit ball in the o?-dimensional Euclidean space. Let s„ denote the length 
of an- 
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Figure 26: An illustration of the covering balls for the A;-nearest PRM* algorithm. The Sn ball is 
guaranteed to contain the balls Bn,m Bn^m+i- 

Lemma 57 For all 61, 92 > 0, 

/ /- \ l/<i 



01 \9i (1 + 1/d + $2) /x(Xfree) / (logn)Vrf " 
In particular, Y.n=i ^i^n) < °° for 6*1, 6*2 > 0. 

Proof Let no G N be a number for which 5n < 8 for all n > no- A bound on the number of balls 
in Bn can computed as follows. For all > no, 



Mn = \Bn\ < 



Sn I (,d \ I ri 



OlQn 01 V(l + l/(^ + ^2)M^free)y Vlogn 

The volume of each ball S„ can be computed as 

logn 



iJi{Bn,m) = Cd{qnf = (1 + ^/d + /^(^free) 



n 



The probability that the ball -B„^^ does not contain a vertex of the fc-nearest PRM* algorithm 
can be bounded as 

Finally, the probability that at least one of the balls in S„ contains no vertex of the /c-nearest 
PRM* can be bounded as 

\ ^ / yn=l 

< ^Jl( <A ^ ( JlJ] ^'^ n-^i+i/d+e,) 

- 01 V(l + l/d + 02)M^free)y Vog fl J 

Q V^' 1 



01 V(l + l/d + 02)M^free)y (logn)Vrf ^1+^2 " 

Clearly, J2n=i ^(^n) < for all 0i, 02 > 0. □ 
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D.5 Construction of the sequence of sets of balls 

Construct a set B'^ = {Bn,i,Bn^2, ■ ■ , Bn,Mn} of balls as B'^ := CoveringBalls((T„, (5„, 9iqn) so that 
each ball in i?^ has radius 6,,, and the spacing between two balls is 9iqn (see Figure 26). 

Clearly, the centers of balls in i?^ coincide with the centers of the balls in i.e., the center 
of B'^ ,^ is the same as the center of Bn^m for all m G {1, 2, . . . , M„} and all n G N. However, the 
balls in B'^ have a larger radius than those in 

D.6 The probability that each ball in B'^ contains at most k{n) vertices 

Recall that the A;- nearest PRM algorithm connects each vertex in the graph with its k{n) nearest 
vertices when the algorithm is run with n samples, where k{n) = kpuM^ogn. Let A'^^ denote the 
event that all balls in B'^ contain at most k{n) vertices of G'^^^^ . 
Recall that A'^ denotes the complement of the event An. 

Lemma 58 ///cprm > e(l + 1/d), then there exists some 61,62 > such that 

Q V^' 1 



^1 V(l + i/d + 62)fi{Xiree) J (logn)V'i n-(i+ei)'*(i+i/d+e2) ■ 
In particular, Yl'^^=i ^i^n) < °° f^^''" some 61,62 > 0. 

Proof Let no G N be a number for which Sn < S for all n > uq. As shown in the proof of Lemma 57, 
the number of balls in B!^ satisfies 

M = \B'\ < ^ = ^( ^ y/y n y/'^ 

' - 6iqn 6i\{l + l/d + 62)fi{Xhee)J VognJ 

For all n > no, the volume of 5^^^ can be computed as 

KB'n,J = Cd {Sn)' = (1 + 6i)'^ (1 + 1/d + 62) M(Xfree) 

Let In,m,i denote the indicator random variable of the event that sample i falls into ball B'^ ^^- 
The expected value of In,m,i can be computed as 

nin,m,i] = = (1 + ^1)'^ (1 + 1/d + 62) 

Let A^n,m denote the number of vertices that fall inside the ball -B^^, i.e., A^n,m = J2i=i ■^n,m,i- 
Then, 

n 

nNn,m]=^nin,mA=nnin,m,l] = (1 + ^^'^(l + 1/d + ^2) log n. 
1=1 

Since {In,m,i}f=i are independent identically distributed random variables, large deviations of their 
sum, Mn^mi can be bounded by the following Chernoff bound (Dubhashi and Panconesi, 2009): 

p({iv.,^>(i + 6)E[iv„,^]}) < ((Y^:^)(iTi)J 

for all e > 0. In particular, for e = e — 1, 
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Since k{n) > e (l+l/d) logn, there exists some 9i,92 > independent of n such that eE[Ar„^fe] = 
e (1 + 61) (1 + 1/c? + 62) logn < k{n). Then, for the same values of Oi and 02, 

P({iV„,^>fc(n)}) < F{{N^,^>eE[N^,m]}) < n-^'+'^^'^'^'/''^'-\ 

Finally, consider the probability of the event that at least one ball in Bn contains more than 
k{n) nodes. Using the union bound together with the inequality above 



U^ll {^n,m > k{n)] ) < F{{Nr,,m > Hfl)}) = M„ F{{Nn,l > k{n)}) 



m=l 



Hence, 



l/d + 02)/i(^free)y (logn)Vrf n-(i+'^i)'(i+i/d+e2) " 
Clearly, Yl^=i'^{^n) < °° ^'^^ the same values of 0\ and 62- □ 

D.7 Connecting the vertices in the subsequent balls in Bn 

First, note the following lemma. 

Lemma 59 If kpuM > e (1 + then there exists 61, 62 > such that the event that each ball 

in Bn contains at least one vertex and each ball in B'n contains at most k{n) vertices occurs for all 
large n, with probability one, i.e., 

¥ fliminf(^„n^;)) = 1. 
\ n->oo / 

Proof Consider the event U A'n, which is the complement of AnCiA'n. Using the union bound, 

F{AlUA'^)<F{Al)+F{A'^). 

Summing both sides, 

00 00 00 

5] F{Ai u A%) < J2 nK) + E < 

n=l n=l n=l 

where the last inequality follows from Lemmas 57 and 58. Then, by the Borel-Cantelli lemma, 
P (limsup„^^(A^ U A'^)) = F (limsup„^^(An n A'nf) = 0, which implies F (liminfn^oo(^n n A'J) = 
1. □ 

Note that for each m G {1, 2, . . . , - 1}, both B 

n,m and Bn^m+i liss entirely inside the ball 
B'n ni (see Figure 26) . Hence, whenever the balls Bn,m and Bn,m+i contain at least one vertex each, 
and B'nni contains at most k{n) vertices, the /c-nearest PRM* algorithm attempts to connect all 
vertices in Bn,m and Bn^m+i with one another. 

The following lemma guarantees that connecting any two points from two consecutive balls in 
Bn results in a collision-free trajectory. The proof of the lemma is essentially the same as that of 
Lemma 54. 

Lemma 60 For all n and all m G {1, 2, . . . , M„}, if Xm G Bn,m o-nd Xm+i G Bn,m+i, then the 
line segment connecting Xm and Xm+i lies in the obstacle-free space, i.e., 

a + (1 - a) Xjn+i e Xfree, for all a G [0, 1]. 



65 



D. 8 Convergence to the optimal path 

The proof of the following lemma is similar to that of Lemma 55, and is omitted here. 

Let Pn denote the set of all paths in the graph returned by /c-PRM* algorithm at the end of n 
iterations. Let o"'^ be the path that is closest to (j„ in terms of the bounded variation norm among 
all those paths in P„, i.e., cr'^ := mino-'gp„ Ho"' — cr„||. 

Lemma 61 The random variable \\a'^ — c7„||bv converges to zero almost surely, i.e., 

P ({lim„_,oo \W'„ - (TuWbv = 0}) = 1. 

A corollary of the lemma above is that lim„_^oo Cn = with probability one. Then, the result 
follows by the robustness of the optimal solution (see the proof of Lemma 56 for details) . 

E Proof of Theorem 36 (Asymptotic optimality of RRG) 

E. l Outline of the proof 

The proof of this theorem is similar to that of Theorem 34. The main difference is the definition 

of Cn that denotes the event that the RRG algorithm has sufficiently explored the obstacle free 
space. More precisely, Cn is the event that for any point x in the obstacle free space, the graph 
maintained by the RRG algorithm algorithm includes a vertex that can be connected to x. 

Construct the sequence {cr„}neN of paths and the sequence {Bn}neN of balls as in the proof of 
Theorem 34. Let An denote the event that each ball in Bn contains a vertex of the graph maintained 
by the RRG by the end of iteration n. Compute n by conditioning on the event that Ci holds for 
alH G { [^3 nj , . . . , n}, where < ^3 < 1 is a constant. Show that the probability that Cj fails to 
occur for any such i is small enough to guarantee that An occurs for all large n with probability 
one. Complete the proof as in the proof of Theorem 34. 

E.2 Definitions of {(T„}„eN and {-B„}neN 

Let ^1 > be a constant. Define Sn, cr„, qn, and Bn as in the proof of Theorem 34. 

E.3 Probability that each ball in Bn contains at least one vertex 

Let An,m be the event that the ball Bn,m contains at least one vertex of the RRG at the end of n 
iterations. Let An be the event that all balls in Bn contain at least one vertex of the RRG at the 
end of iteration n, i.e., An = C\m=i ^n,m, where M„ is the number of balls in Bn- Recall that 7rrg 
is the constant used in defining the connection radius of the RRG algorithm (see Algorithm 5). 

Lemma 62 //7rrg > 2(1 + l/d)^^"^ ^ m(^&cc) j ^^^^ there exists 61 > such that An occurs for 
all large n with probability one, i.e., 

P(liminf„^oo An) = I. 

The proof of this lemma requires two intermediate results, which are provided next. 

Recall that ij is the parameter used in the Steer procedure (see the definition of Steer procedure 
in Section 3.1). Let C„ denote the event that for any point x G Xfree, the graph returned by the 

RRG algorithm includes a vertex v such that — f || < r/ and the line segment joining v and x is 
collision-free. The following lemma establishes an bound on the probability that this event fails to 
occur at iteration n. 
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Lemma 63 There exists constants a,b e M>o such that P{C^) < ae for all n G N. 

Proof Partition Xfj-ge into finitely many convex sets such that each partition is bounded by a ball 
a radius rj. Such a finite partition exists by the boundedness of X^ee. Denote this partition by 
X[, X2, . . . , X'j^. Since the probability of failure decays to zero with an exponential rate, for any 
m G {1,2,..., M}, the probability that X'^ fails to contain a vertex of the RRG decays to zero 
with an exponential rate, i.e., 

P {{$x e n x'^}) < am e-''-" 

The probability that at least one partition fails to contain one vertex of the RRG also decays to 
zero with an exponential rate. That is, there exists a, 6 G M>o such that 

^ ^ m=l m=l 

where the first inequality follows from the union bound. □ 

Let < ^3 < 1 be a constant independent of n. Consider the event that Q occurs for all i that 
is greater than O^n, i.e., nr=[93nj ^i- following lemma analyzes the probability of the event 
that (^^^[esnj ^^^^ *° occur. 

Lemma 64 For any 63 G (0, 1), 

£K(n:...'^.)l<- 

n=l ' ' 

Proof The following inequalities hold: 

i:r((n;i,.„jc. ) = i:r(u:i,.„jcO ^ y. y. nc?) < ^ e 

n=l '--'// n=l ^ >- J / n=li=[03i] n=lj=[6»3nj 

where the last inequality follows from Lemma 63. The right-hand side is finite for all a,b > 0. □ 

Proof of Lemma 62 It is shown that Yl'i^=i ^ i-^n) < which, by the Borel-Cantelli Lemma (Grim- 
mett and Stirzaker, 2001), implies that A'^ occurs infinitely often with probability zero, i.e., 
P(limsup„^go ^5^) = 0, which in turn implies P(liminf„^oo ^n) = 1- 

Let no G N be a number for which Sn < S for all n > hq. First, for all n > Hq, the number of 
balls in can be bounded by (see the proof of Lemma 52 for details) 



Second, for all n > no, the volume of each ball in Bn can be calculated as (see the proof of 
Lemma 52) 

n(R ^ - t i TpRm V logn 
where is the volume of the unit ball in the d-dimensional Euclidean space. 
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Third, conditioning on the event Ci, each new sample will be added to the graph 

maintained by the RRG algorithm as a new vertex between iterations i = [63 n\ and i = n. Thus, 



403 nj J \ IJ,{Xiree) J \ KXhcc] 

< ( 



Cd f 7RRG \ log n \ 



/x(Xfree) + ^1/ n J 

(l-e3)Cd / 7RRG ^'^W„ (l-g3)Cd / 7RRG V 

where the fourth inequality follows from (1 — l//(n))^(") < e^^")/-^^"^. 
Fourth, 

^s|n:,.„jc<) < F(u:':.<.|n:i,.„jC.) 

TO=1 ^ ' 



(2 + 0i)s„/ n N^/'' _ (1-^3) Cd(- mac 
6'i7RRG VognJ 



Hence, 



n=l ^ ^ 

whenever ^^gj (ir^)' - > 1, i.e., trrg > (2 + 0i)(l + (^^(^y^'', which is 



satisfied by appropriately choosing the constants Oi and ^3, since 7rrg > 2 (l+l/d)^/'' ^ /^(^faee) ^ 
Finally, 



= i-P(A.u(nr=L,3njC.r) 
> i-P(A„)-P((nr=L,3njCir) 

= p(^;;)-p(K=Le3„ja)^). 



Taking the infinite sum of both sides yields 



j:nAi) < j:f [ai I f]i c„ + i:K(n:,.„j ) ■ 

n=l n=l ^ ' n=l ^ ^ ' ' 

The first term on the right hand side is shown to be finite above. The second term is finite by 
Lemma 64. Hence, YlnLi ^i^n) < 00. Then, by the Borel Cantelli lemma, yl^ occurs infinitely 
often with probability zero, which implies that its complement An occurs for all large n, with 
probability one. □ 
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E.4 Convergence to the optimal path 

The proof of the following lemma is similar to that of Lemma 55, and is omitted here. 

Let Pn denote the set of all paths in the graph returned by RRG algorithm at the end of n 
iterations. Let o"'^ be the path that is closest to (j„ in terms of the bounded variation norm among 
all those paths in P„, i.e., a'^ := mino-'gp„ Ho"' — crn||. 

Lemma 65 The random variable \\a'^ — crn||BV converges to zero almost surely, i.e., 

P ({lim„^.oo IWn - C^nllBV = O}) = 1. 

A corollary of the lemma above is that lim^^oo = <7* with probability one. Then, the result 
follows by the robustness of the optimal solution (see the proof of Lemma 56 for details) . 



F Proof of Theorem 37 (asymptotic optimality of /c-nearest RRG) 
F.l Outline of the proof 

The proof of this theorem is a combination of that of Theorem 35 and 36. 

Define the sequences {(Tn}neN, {-BnjneN) and {i^^j^gN as in the proof of Theorem 35. Define 
the event C„ as in the proof of Theorem 36. Let denote the event that each ball in 5„ contains 
at least one vertex, and ^4^ denote the event that each ball in B'^ contains at most k{n) vertices of 
the graph maintained by the RRG algorithm, by the end of iteration n. Compute An and by 
conditioning on the event that Ci holds for all i = O^n to n. Show that this is enough to guarantee 
that An and A'^ hold together for all large n, with probability one. 

F.2 Definitions of {c7n}neN, {-BnjneN, and {-B^}n6N 

Let 01, ^2 > be two constants. Define 6n, cr„, Qn, Bn, and B'^ as in the proof of Theorem 35. 



F.3 The probability that each ball in Bn contains at least one vertex 
Let A 

n,m denote the event that the ball B^^m contains at least one vertex of the graph maintained 
by the fe-nearest RRG algorithm by the end of iteration n. Let An denote the event that all balls 
in Bn,m contain at least one vertex of the same graph, i.e.. An = Um=i ■^n,m- Let s„ denote the 
length of (Tn, i-e., TV {an). Recall 77 is the parameter in the Steer procedure. Let Cn denote the 
event that for any point x G Xfree, the A;- nearest RRG algorithm includes a vertex v such that 
\\x — v\\ < rj. 

Lemma 66 For any ^1,^2 > and any 9s G (0, 1), 

i/d 1 



"I I \i=i9sni 7 - 9i\9i{l + l/d + 92)n{Xf,^e)J (logn)Vd n(i-«3)(i+i/d+e2)-i/<i- 
In particular, Yl'^=i ^i^n I nr=[e»3nj ^ f^''' ^''^V ^2 > and some 9^ G (0, 1). 



Proof Let no G N be a number for which 6n < S for all n> uq. Then, for all n > no, 

Mn = \Bn\ < 



/ /- \ V"' / \ ^/d 



OiQn 9x\{\ + l/d + 92)^l{Xi,^)) \\ogn 
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The volume of each ball B„ can be computed as 

fl{Bn,m) = CdiQnf = (1 + 1/d + 62) //(Xfree)^- 



Given f]"^ ^-| Cj , the probability that the ball Bn,m does not contain a vertex of the A;-nearest 
PRM* algorithm can be bounded as 

Finally, the probability that at least one of the balls in B„ contains no vertex of the /c-nearest 
PRM* can be bounded as 

/ M N M„ 

/ /- \ l/<^ 1 

^ Sn ( Cd \ 1 



Clearly, for all 01,02 > 0, there exists some ^3 G (0, 1) such that Yl'^=i ^i^n) ^ 

F.4 The probability that each ball in B'^ contains at most k{n) vertices 

Let A'^ denote the event that all balls in B'^ contain at most k(n) vertices of the graph maintained 
by the RRG algorithm, by end of iteration n. 

Lemma 67 ///cprm > e(l + 1/d), then there exists 0i,02,03 > such that 

fIa'^IcT < ^ V^'^ - 

\^ "|l \i=[esn} 7 - 01 \{l + l/d + 02)fi{Xf,^e)J (iogn)V<i n-(i-^3)(i+ei)<^(i+i/d+e2)- 

In particular, Yl'^=i ^i-^n I nr=[e3nj ^ f^''~ ^ome 0i,02 > and some 03 > 0. 

Proof Let no G N be a number for which Xn < S for all n > uq. Then, the number of balls in 
and the volume of each ball can be computed as 

Mr, = \B'J < ' \ ( ^ 



' - OlQn 01 \il + l/d + 92)liiXi,,,)J \\ogn 
KB'n,m) = Cd (A„)'^ = (1 + 0l)' (1 + 1/d + 02) /x(Xfree) 

Let In,m,i denote the indicator random variable of the event that sample i falls into ball B'^^^. 
The expected value of In,m,i can be computed as 

nin,mA = = (1 + ^l)'^ (1 + 1/d + 02)^^. 

Let Nji^jn denote the number of vertices that fall inside the ball B!^ ,^ between iterations [^3 n\ and 
n, i.e., Nn,m = Yd=[e3n} In,m,i- Then, 

n 

nNn,m] = Wn,mA = (1 - ^3) n E[/„,„,i] = (1 - ^3) (1 + ^i)"' (1 + 1/d + ^2) logn. 

i=\_e3n\ 



70 



Since {In,m,i}f=i Si-ice independent identically distributed random variables, large deviations of their 
sum, Mn,m, can be bounded by the following Chernoff bound (Dubhashi and Panconesi, 2009): 

P( { iV„,„ > (1 + e)E[N^,m] } ) < ((i^^J 
for all e > 0. In particular, for e = e — 1, 

F{{Nn,m>enNn,m]}) < e-"^!^"-! = „-a-^3)(i+^i)'*(i+i/d+(?2). 

Since k{n) > e(l + 1/d) logn, there exists some ^1,^2 > and 63 G (0,1), independent of n, 
such that eE[Nn,k] = e (1 - 6*3) (1 + 0i) (1 + l/c/ + 6*2) logn < k{n). Then, for the same values of 6*1 
and 02, 

P( { iV„,^ > fc(n) } ) < F{{N^,^>eE[N^,m]}) < ^-d-^a) d+^i)'^ (1+1/^^+^2). 

Finally, consider the probability of the event that at least one ball in S„ contains more than 
k{n) nodes. Using the union bound together with the inequality above 



[jS^,{^n,m>k{n)}] < J2H{^n,m>Hn)}) = M„ P({iV„,i > A;(n)}) 



m=l 



Hence, 



'(<in:H.,„jC-) = ••(u^i'v-,™ >*(«)}) 



^ I Cd 



1/d 



01 \{l + l/d + 02)n{Xi,^e)J (logny/d n-i^-es){i+e,ni+i/d+e2)- 
Clearly, E^=i ^(^n I ^7=[e3n} ^i) < °° ^he same values of 0i, 02, and 0^. □. 
F.5 Connecting the vertices in subsequent balls in S„ 

Lemma 68 // /cprm > e (1 + l/d)^/'^, then there exists 0i,02 > such that the event that each ball 
in Bn contains at least one vertex and each hall in B'^ contains at most k{n) vertices occurs for all 
large n, with probability one, i.e., 

pfliminf(A„nO') =1. 

First note the following lemma. 
Lemma 69 For any 03 G (0, 1), 



E<(n:,.,^.)l < 

n=l ^ ^ ' ' 



00. 



Proof Since the RRG algorithm and the fc- nearest RRG algorithm have the same vertex sets, i.e., 
V^^*^ = V^Rf^*^ surely for all n G N, the lemma follows from Lemma 64. □ 
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Proof of Lemma 68 Note that 

where the last inequahty follows from the union bound. Rearranging and using the union bound, 

p(A^u<^) < pKI n^=L^3„jC0+PK| n^=L^3„jQ)+P((n^=Le3„jC,)^). 

Summing both sides, 

n=l n=l ^ ^ n=l ^ ^ n=l ^ ^ ^ ^ 

where the right hand side is finite by Lemmas 66, 67, and 69, by picking ^3 close to one. Hence, 
X;^=iP(^n U A'^) < 00. Then, by the Borel-Cantelh lemma, P(limsup„^^(A^ U A'^)) = 0, or 
equivalently P(lim infn^oo(^n n A'^)) = 1. □ 

F. 6 Convergence to the optimal path 

The proof of the following two lemmas are essentially the same as that of Lemma 55, and is omitted 
here. Let P„ denote the set of all paths in the graph returned by /c-RRG algorithm at the end of n 
iterations. Let be the path that is closest to an in terms of the bounded variation norm among 
all those paths in P„, i.e., cr^ := ming-rgp^ \\a' — cr„||. 

Lemma 70 The random variable ||(T^ — <t„||bv converges to zero almost surely, i.e., 

P ({lim„_^oo Wcr'n - o-nllsv = 0}) = 1. 

A corollary of the lemma above is that lim^^oo o"n = with probability one. Then, the result 
follows by the robustness of the optimal solution (see the proof of Lemma 56 for details) . 

G Proof of Theorem 38 (Asymptotic optimality of RRT*) 

For simplicity, the proof will assume the steering parameter 77 to be large enough, i.e., rj > diam(Af), 
although the results hold for any 77 > 0. 

G. l Marked point process 

Consider the following marked point process. Let {Xi, X2, ■ ■ . , Xn} be a independent uniformly 
distributed points drawn from Xf^^e and let {Yi,Y2, . . . ,Yn} be independent uniform random vari- 
ables with support [0, 1]. Each point Xi is associated with a mark Yi that describes the order of 
Xi in the process. More precisely, a point Xi is assumed to be drawn after another point Xi' if 
Yi' <Yi. Wc will also assume that the point process includes the point Xmit with mark Y = 0. 

Consider the graph formed by adding an edge {Xi',Xi), whenever (i) Yi' < Yi and (ii) — 
-'^i'll ^ i^n both hold. Notice that, formed in this way, G„ includes no directed cycles. Denote this 
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graph by G„ = {Vn,En). Also, consider a subgraph G'^ of G„ formed as follows. Let c(Xj) denote 
the cost of best path starting from Xinit and reaching Xj. In G'j, each vertex Xi has a single parent 
Xj with the smallest cost c(Xj). Since the graph is built incrementally, the cost of the best path 
reaching Xj will be the same as the one reaching Xj/ in both Gn and G^. Clearly, G'^ is equivalent 
to the graph returned by the RRT* algorithm at the end of n iterations, if the steering parameter 
7/ is large enough. 

Let Yn and the denote the costs of the best paths starting from Xinit and reaching the goal 
region in Gn and G'^, respectively. Then, limsup^^g^y^ = limsup^^oQy^ surely. In the rest of 
the proof, it is shown that P({limsup„_^(^ Y^}) = 1, which implies that P({limsup„_^(^ Y^}) = 1, 
which in turn implies the result. 

G.2 Definitions of {(J„}„gN and {Bn}neN 
Let a* denote an optimal path. Define 

6n ■■= min{(5,4r„}, 

where r„ is the connection radius of the RRT* algorithm. Let {(Tn}neN be the sequence paths, the 
existence of which is guaranteed by Lemma 50. 

For each n G N, construct a sequence {Bn}neN of balls that cover a^, as Bn = {Bn,i, Bn,2, ■ ■ ■ , Bn,M„} '■= 
CoveringBalls((Tn,'"n,2r,i) (see Definition 51), where rn is the connection radius of the RRT* al- 

gorithm, i.e., = 7rrt* ( "^f^ ) • Clearly, the balls in B^ are openly disjoint, since the spacing 
between any two consecutive balls is 2r„. 



G.3 Connecting the vertices in subsequent balls in Bn 

For all m G {1,2,..., M„}, let An,m denote the event that there exists two vertices Xi, Xj/ G ^f''^"'" 
such that Xj G Bn,m, Xj/ G Bn,m+i and Yj' < Ij, where Yi and Ij/ are the marks associated with 
points Xj and Xj/, respectively. Notice that, in this case, Xj and Xj/ will be connected with an edge 
in Gn- Let An denote the event that ^n,m holds for all m G {1, 2, . . . , M}, i.e.. An = {Xn=i ■^n,m- 

Lemma 71 If ^rrt* > 4 ; then An occurs for all large n, with probability one, i.e., 

P fliminf yl^) = 1. 
\ n->oo / 

Proof The proof of this result is based on a Poissonization argument. Let Poisson(A) be a Poisson 
random variable with parameter X = 6n, where 9 G (0, 1) is a constant independent of n. Consider 
the point process that consists of exactly Poisson(0n) points, i.e., {Xi,X2, . . . , Xpoisgon(6)n)}- This 
point process is a Poisson point process with intensity 9n ///(Xfree) by Lemma 11. 

Let An^m denote the event that there exists two vertices Xj and Xj/ in the vertex set of the 
RRT* algorithm such that X; and Xj/ are connected with an edge in Gn, where G„ is the graph 
returned by the RRT* when the algorithm is run for Poisson(0n) many iterations, i.e., Poisson(0n) 
samples are drawn from Xf^ee- 

Clearly, P(A^,^) = P(i^,„ I {Poisson(6' n) = n}). Moreover, 

P(A^,„) < P(i^,„)+P({Poisson(^n) >n}). 

since P(yl^^) is non-increasing with n (see, e.g., Penrose, 2003). Since ^ < 1, P({Poisson(^ n) > 
n}) < e"""", where a > is a constant independent of n. 
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To compute P(y4$^ „j), a number of definitions are provided. Let Nn,m denote the number of 
vertices that he in the interior of Bn,m- Clearly, E[Ar„ „j] = ''l^^x^'^) n, for all m G {1,2,..., M„}. 

For notational simplicity, define a := ^(^xP'^) ■ ^ ^ ^) ^ constant independent of n. Define 
the event 

Cn,m,e := {Nn,m > (1 - f) K[Af„,^]} = {Nn,m > (1 - e) « log n} 

Since Nn,m,e is binomially distributed, its large deviations from its mean can be bounded as fol- 
lows (Penrose, 2003), 

P (C;;,^,,) = n{Nn,m,e < (1 - 6)E[iV„,^]}) < e-"^(^) '^^^ = n'^^^, 

where i?(e) = e + (1 — e) log(l — e). Notice that H{e) is a continuous function of e with -ff(O) = 
and H{\) = 1. Hence, H{e) can be made arbitrary close to one by taking e close to one. 
Then, 

^(^n,m) = ^(^n,m I C'n,m,e H Cn,^+i,g) P(C„,„i,e Cl Cn,m+l,e) 

^ ^{^n,m I C'n,m,e H Cn,m+l,e) P(Cn,m,e H Cn,m+l,e) + P(Cn,m,e) + ^{Cn,m+l,e)^ 

where the last inequality follows from the union bound. 

First, using the spatial independence of the underlying point process, 

P {Cn,m,e H Cn,m+l,e) = P {Cn,m,e) P {Cn,m+l,e) < n~^°'^^^\ 

Second, observe that P(^^ „ | Nn^m = N^^m+i = k') is a non-increasing function of both k 
and k' , since the probability of the event Ari,m can not increase with the increasing number of 
points in both balls, Bn,m and Bn,m+i- Then, 

P(i^^„ I Cn,m,e H Cn,m+l,e) = P(^n,m I {Nn,rn > (1 - e) a log Nn,m, Nn,m+1 > (1 - e) a log Nn,m+1 

< P(i^,^ I {Nn,m = (1 - e) a log Nn,m, A^n,m+1 = (1 - e) Ct log Nn,m+1 

The term on the right hand side is one minus the probability that the maximum of a \ogn 
number of uniform samples drawn from [0, 1] is smaller than the minimum of a log n number of 
samples again drawn from [0, 1], where all the samples are drawn independently. This probability 
can be calculated as follows. From the order statistics of uniform distribution, the minimum 
of a logn points sampled independently and uniformly from [0,1] has the following probability 
distribution function: 

(^1 _ x)"' logn-l 

■^"^'"^^^ " Beta(l,«log(n))' 

where Beta(-, •) is the Beta function (also called the Euler integral) (Abramowitz and Stegun, 1964). 
The maximum of the same number of independent uniformly distributed random variables with 
support [0, 1] has the following cumulative distribution function: 
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< 



Then, 

JO 

Gaiimia((l — e) a logn) Gainma((l — e) e logn) 

2 Gainma(2(l - e) a log(n)) 
((1 — e)a logn)! ((1 — e) a logra)! 
2(2(1 - e) a logn)! 

((1 — e)a logn)! 
2(2(l-e)a logn)(2(l-e)a logn - 1)---1 

— 2(1— e) ot logn ' 

where Gamma(-) is the gamma function (Abramowitz and Stegun, 1964). 
Then, 



nKm) < n-°(2^(^)+'°^(2)(^-^)) +2n-"^(^). 

Since 2iJ(e) + log(2) (1 — e) and H{e) are both continuous and increasing in the interval (0.5, 1), 
the former is equal to 2 — log(4) > 0.5 and the latter is equal to 1 as e approaches one from below, 
there exists some e G (0.5, 1) such that both 2H{e) + log(2) (1 - e) > 0.5 and iJ(e) > 0.5. Thus, 

Hence, 

nK,m) < P(i^,„)+P(Poisson(0n)>n) 
< 3n""/^ + e""'* 

Recall that An denotes the event that An^m holds for all m G {1, 2, . . . , M„}. Then, 

where the last inequality follows from the union bound. The number of balls in B^, can be bounded 
as 



\Bn\ = M„ < /3 



, l/d 



log n ^ 

where /? is a constant. Combining this with the inequality above, 

l/d 



which is summable for a > 2 (1 + l/d). Thus, by the Borel-Cantelli lemma, the probability that 

A^ occurs infinitely often is zero, i.e., P(lim sup„_i.oQ ^n) — 0) which implies that A^ occurs for all 
large n with probability one, i.e., P(liminf„^oo ^n) = 1- C 
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G.4 Convergence to the optimal path 

The proof of the following lemma is similar to that of Lemma 55, and is omitted here. 

Let Pn denote the set of all paths in the graph returned by RRT* algorithm at the end of n 
iterations. Let o"'^ be the path that is closest to o"„ in terms of the bounded variation norm among 
all those paths in P„, i.e., a'^ := mino-'gp„ Ho"' — crn||. 

Lemma 72 The random variable ||cr^ — crn||BV converges to zero almost surely, i.e., 

P ({lim„^.oo - ct^IIbv = O}) = 1. 

A corollary of the lemma above is that lim^^oo = <7* with probability one. Then, the result 
follows by the robustness of the optimal solution (see the proof of Lemma 56 for details) . 
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